sensor.h

Go to the documentation of this file.

/* HokuyoAIST
 *
 * Header file for the sensor object.
 *
 * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp
 *   RT-Synthesis Research Group
 *   Intelligent Systems Research Institute,
 *   National Institute of Advanced Industrial Science and Technology (AIST),
 *   Japan
 *   All rights reserved.
 *
 * This file is part of HokuyoAIST.
 *
 * HokuyoAIST is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published
 * by the Free Software Foundation; either version 2.1 of the License,
 * or (at your option) any later version.
 *
 * HokuyoAIST is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with HokuyoAIST. If not, see
 * <http://www.gnu.org/licenses/>.
 */

#ifndef INCLUDE_HOKUYOAIST_SENSOR_H__
#define INCLUDE_HOKUYOAIST_SENSOR_H__

#include <string>

#if defined(WIN32)
  typedef unsigned char       uint8_t;
  typedef unsigned int      uint32_t;
  #if defined(HOKUYOAIST_STATIC)
    #define HOKUYOAIST_EXPORT
  #elif defined(hokuyoaist_EXPORTS)
    #define HOKUYOAIST_EXPORT     __declspec(dllexport)
  #else
    #define HOKUYOAIST_EXPORT     __declspec(dllimport)
  #endif
#else
  #include <stdint.h>
  #define HOKUYOAIST_EXPORT
#endif

namespace flexiport {
  class Port;
}

namespace hokuyoaist {

enum MultiechoMode
{
  ME_OFF,
  ME_FRONT,
  ME_MIDDLE,
  ME_REAR,
  ME_AVERAGE
};


HOKUYOAIST_EXPORT inline char const* multiecho_mode_to_string(MultiechoMode mode)
{
  switch (mode) {
    case ME_OFF:
      return "Off";
    case ME_FRONT:
      return "Front";
    case ME_MIDDLE:
      return "Middle";
    case ME_REAR:
      return "Rear";
    case ME_AVERAGE:
      return "Average";
    default:
      return "Unknown";
  }
}


typedef struct IPAddr
{
  unsigned int first;
  unsigned int second;
  unsigned int third;
  unsigned int fourth;
} IPAddr;


class HOKUYOAIST_EXPORT Sensor
{
  public:
    Sensor();
    explicit Sensor(std::ostream& err_output);
    ~Sensor();

    void open(std::string port_options);

    void close();

    bool is_open() const;

    void set_power(bool on);

    void set_ip(IPAddr const& addr, IPAddr const& subnet, IPAddr const& gateway);

    void reset();

    void semi_reset();

    void set_motor_speed(unsigned int speed);

    void set_high_sensitivity(bool on);

    void get_sensor_info(SensorInfo& info);

    unsigned long long get_time();

    unsigned int get_raw_time();

    long long calibrate_time(unsigned int skew_sleep_time=0, unsigned int samples=10);

    long long time_offset() const
    {
      return time_offset_;
    }

    void set_time_offset(long long time_offset)
    {
      time_offset_ = time_offset;
    }
    float drift_rate() const
    {
      return time_drift_rate_;
    }
    void set_drift_rate(float drift_rate)
    {
      time_drift_rate_ = drift_rate;
    }

    float skew_alpha() const
    {
      return time_skew_alpha_;
    }
    void set_skew_alpha(float alpha)
    {
      time_skew_alpha_ = alpha;
    }

    unsigned int get_ranges(
      ScanData& data,
      int start_step = -1,
      int end_step = -1,
      unsigned int cluster_count = 1);

    unsigned int get_ranges_by_angle(
      ScanData& data,
      double start_angle,
      double end_angle,
      unsigned int cluster_count = 1);

    unsigned int get_ranges_intensities(
      ScanData& data,
      int start_step = -1,
      int end_step = -1,
      unsigned int cluster_count = 1);

    unsigned int get_ranges_intensities_by_angle(
      ScanData& data,
      double start_angle,
      double end_angle,
      unsigned int cluster_count = 1);

    unsigned int get_new_ranges(
      ScanData& data,
      int start_step = -1,
      int end_step = -1,
      unsigned int cluster_count = 1);

    unsigned int get_new_ranges_by_angle(
      ScanData& data,
      double start_angle,
      double end_angle,
      unsigned int cluster_count = 1);

    unsigned int get_new_ranges_intensities(
      ScanData& data,
      int start_step = -1,
      int end_step = -1,
      unsigned int cluster_count = 1);

    unsigned int get_new_ranges_intensities_by_angle(
      ScanData& data,
      double start_angle,
      double end_angle,
      unsigned int cluster_count = 1);

    uint8_t scip_version() const      { return scip_version_; }

    void set_verbose(bool verbose)      { verbose_ = verbose; }

    void ignore_unknowns(bool ignore)     { ignore_unknowns_ = ignore; }

    void set_multiecho_mode(MultiechoMode mode) { multiecho_mode_ = mode; }

    double step_to_angle(unsigned int step);
    unsigned int angle_to_step(double angle);

  private:
    flexiport::Port* port_;
    std::ostream& err_output_;

    uint8_t scip_version_;
    LaserModel model_;
    bool verbose_, enable_checksum_workaround_, ignore_unknowns_;
    MultiechoMode multiecho_mode_;
    double min_angle_, max_angle_, resolution_;
    int first_step_, last_step_, front_step_;
    unsigned int max_range_;
    unsigned int time_resolution_;
    long long time_offset_;
    unsigned int last_timestamp_;
    unsigned int wrap_count_;
    float time_drift_rate_;
    float time_skew_alpha_;

    void clear_read_buffer();
    int read_line(char* buffer, int expected_length=-1);
    int read_line_with_check(char* buffer, int expected_length=-1, bool has_semicolon=false);
    bool read_data_block(char* buffer, int& block_size);
    void skip_lines(int count);
    int send_command(char const* cmd, char const* param, int param_length, char const* extra_ok);

    void enter_timing_mode();
    void leave_timing_mode();
    unsigned int get_timing_mode_time(unsigned long long* reception_time=0);
    unsigned long long get_computer_time();
    unsigned int wrap_timestamp(unsigned int timestamp);
    unsigned long long offset_timestamp(unsigned int timestamp);
    unsigned int step_to_time_offset(int start_step);

    void find_model(char const* buffer);
    void get_and_set_scip_version();
    void get_defaults();
    void process_vv_line(char const* buffer, SensorInfo& info);
    void process_pp_line(char const* buffer, SensorInfo& info);
    void process_ii_line(char const* buffer, SensorInfo& info);

    uint32_t process_echo_buffer(int const* buffer, int num_echos);
    void read_2_byte_range_data(ScanData& data, unsigned int num_steps);
    void read_3_byte_range_data(ScanData& data, unsigned int num_steps);
    void read_3_byte_range_and_intensity_data(ScanData& data, unsigned int num_steps);

    int confirm_checksum(char const* buffer, int length, int expected_sum);
}; // class Sensor

} // namespace hokuyoaist

#endif // INCLUDE_HOKUYOAIST_SENSOR_H__