HokuyoAIST
3.0.1
|
00001 /* HokuyoAIST 00002 * 00003 * Header file for the sensor object. 00004 * 00005 * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp 00006 * RT-Synthesis Research Group 00007 * Intelligent Systems Research Institute, 00008 * National Institute of Advanced Industrial Science and Technology (AIST), 00009 * Japan 00010 * All rights reserved. 00011 * 00012 * This file is part of HokuyoAIST. 00013 * 00014 * HokuyoAIST is free software; you can redistribute it and/or modify it 00015 * under the terms of the GNU Lesser General Public License as published 00016 * by the Free Software Foundation; either version 2.1 of the License, 00017 * or (at your option) any later version. 00018 * 00019 * HokuyoAIST is distributed in the hope that it will be useful, but 00020 * WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00022 * Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public 00025 * License along with HokuyoAIST. If not, see 00026 * <http://www.gnu.org/licenses/>. 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 }; // class Sensor 00530 00531 } // namespace hokuyoaist 00532 00535 #endif // SENSOR_H__ 00536