00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef SENSOR_H__
00030 #define SENSOR_H__
00031
00032 #include <string>
00033
00034 #if defined(WIN32)
00035 typedef unsigned char uint8_t;
00036 typedef unsigned int uint32_t;
00037 #if defined(HOKUYOAIST_STATIC)
00038 #define HOKUYOAIST_EXPORT
00039 #elif defined(hokuyoaist_EXPORTS)
00040 #define HOKUYOAIST_EXPORT __declspec(dllexport)
00041 #else
00042 #define HOKUYOAIST_EXPORT __declspec(dllimport)
00043 #endif
00044 #else
00045 #include <stdint.h>
00046 #define HOKUYOAIST_EXPORT
00047 #endif
00048
00049 namespace flexiport
00050 {
00051 class Port;
00052 }
00053
00058 namespace hokuyoaist
00059 {
00060
00073 enum MultiechoMode
00074 {
00075 ME_OFF,
00076 ME_FRONT,
00077 ME_MIDDLE,
00078 ME_REAR,
00079 ME_AVERAGE
00080 };
00081
00082
00083 HOKUYOAIST_EXPORT inline char const* multiecho_mode_to_string(MultiechoMode mode)
00084 {
00085 switch(mode)
00086 {
00087 case ME_OFF:
00088 return "Off";
00089 case ME_FRONT:
00090 return "Front";
00091 case ME_MIDDLE:
00092 return "Middle";
00093 case ME_REAR:
00094 return "Rear";
00095 case ME_AVERAGE:
00096 return "Average";
00097 default:
00098 return "Unknown";
00099 }
00100 }
00101
00102
00104 typedef struct IPAddr
00105 {
00107 unsigned int first;
00109 unsigned int second;
00111 unsigned int third;
00113 unsigned int fourth;
00114 } IPAddr;
00115
00116
00130 class HOKUYOAIST_EXPORT Sensor
00131 {
00132 public:
00133 Sensor();
00134 Sensor(std::ostream& err_output);
00135 ~Sensor();
00136
00138 void open(std::string port_options);
00139
00141 void close();
00142
00144 bool is_open() const;
00145
00147 void set_power(bool on);
00148
00155 void set_ip(IPAddr const& addr, IPAddr const& subnet,
00156 IPAddr const& gateway);
00157
00159 void reset();
00160
00164 void semi_reset();
00165
00175 void set_motor_speed(unsigned int speed);
00176
00179 void set_high_sensitivity(bool on);
00180
00182 void get_sensor_info(SensorInfo& info);
00183
00185 unsigned long long get_time();
00186
00188 unsigned int get_raw_time();
00189
00218 long long calibrate_time(unsigned int skew_sleep_time=0,
00219 unsigned int samples=10);
00220
00222 long long time_offset() const { return time_offset_; }
00223
00225 void set_time_offset(long long time_offset)
00226 { time_offset_ = time_offset; }
00228 float drift_rate() const { return time_drift_rate_; }
00244 void set_drift_rate(float drift_rate)
00245 { time_drift_rate_ = drift_rate; }
00246
00248 float skew_alpha() const { return time_skew_alpha_; }
00261 void set_skew_alpha(float alpha) { time_skew_alpha_ = alpha; }
00262
00285 unsigned int get_ranges(ScanData& data, int start_step = -1,
00286 int end_step = -1, unsigned int cluster_count = 1);
00287
00302 unsigned int get_ranges_by_angle(ScanData& data, double start_angle,
00303 double end_angle, unsigned int cluster_count = 1);
00304
00327 unsigned int get_ranges_intensities(ScanData& data,
00328 int start_step = -1, int end_step = -1,
00329 unsigned int cluster_count = 1);
00330
00345 unsigned int get_ranges_intensities_by_angle(ScanData& data,
00346 double start_angle, double end_angle,
00347 unsigned int cluster_count = 1);
00348
00375 unsigned int get_new_ranges(ScanData& data, int start_step = -1,
00376 int end_step = -1, unsigned int cluster_count = 1);
00377
00392 unsigned int get_new_ranges_by_angle(ScanData& data,
00393 double start_angle, double end_angle,
00394 unsigned int cluster_count = 1);
00395
00421 unsigned int get_new_ranges_intensities(ScanData& data,
00422 int start_step = -1, int end_step = -1,
00423 unsigned int cluster_count = 1);
00424
00439 unsigned int get_new_ranges_intensities_by_angle(ScanData& data,
00440 double start_angle, double end_angle,
00441 unsigned int cluster_count = 1);
00442
00444 uint8_t scip_version() const { return scip_version_; }
00445
00448 void set_verbose(bool verbose) { verbose_ = verbose; }
00449
00452 void ignore_unknowns(bool ignore) { ignore_unknowns_ = ignore; }
00453
00455 void set_multiecho_mode(MultiechoMode mode) { multiecho_mode_ = mode; }
00456
00458 double step_to_angle(unsigned int step);
00461 unsigned int angle_to_step(double angle);
00462
00463 private:
00464 flexiport::Port* port_;
00465 std::ostream& err_output_;
00466
00467 uint8_t scip_version_;
00468 LaserModel model_;
00469 bool verbose_, enable_checksum_workaround_,
00470 ignore_unknowns_;
00471 MultiechoMode multiecho_mode_;
00472 double min_angle_, max_angle_, resolution_;
00473 int first_step_, last_step_, front_step_;
00474 unsigned int max_range_;
00476 unsigned int time_resolution_;
00479 long long time_offset_;
00482 unsigned int last_timestamp_;
00484 unsigned int wrap_count_;
00486 float time_drift_rate_;
00488 float time_skew_alpha_;
00489
00490 void clear_read_buffer();
00491 int read_line(char* buffer, int expected_length=-1);
00492 int read_line_with_check(char* buffer, int expected_length=-1,
00493 bool has_semicolon=false);
00494 bool read_data_block(char* buffer, int& block_size);
00495 void skip_lines(int count);
00496 int send_command(char const* cmd, char const* param, int param_length,
00497 char const* extra_ok);
00498
00499 void enter_timing_mode();
00500 void leave_timing_mode();
00502 unsigned int get_timing_mode_time(unsigned long long* reception_time=0);
00504 unsigned long long get_computer_time();
00506 unsigned int wrap_timestamp(unsigned int timestamp);
00509 unsigned long long offset_timestamp(unsigned int timestamp);
00512 unsigned int step_to_time_offset(int start_step);
00513
00514 void find_model(char const* buffer);
00515 void get_and_set_scip_version();
00516 void get_defaults();
00517 void process_vv_line(char const* buffer, SensorInfo& info);
00518 void process_pp_line(char const* buffer, SensorInfo& info);
00519 void process_ii_line(char const* buffer, SensorInfo& info);
00520
00521 uint32_t process_echo_buffer(int const* buffer, int num_echos);
00522 void read_2_byte_range_data(ScanData& data, unsigned int num_steps);
00523 void read_3_byte_range_data(ScanData& data, unsigned int num_steps);
00524 void read_3_byte_range_and_intensity_data(ScanData& data,
00525 unsigned int num_steps);
00526
00527 int confirm_checksum(char const* buffer, int length,
00528 int expected_sum);
00529 };
00530
00531 }
00532
00535 #endif // SENSOR_H__
00536