From 6504f62bbba1c64849e3c1800051b9a3675899ec Mon Sep 17 00:00:00 2001 From: Matous Pokorny Date: Sun, 21 Sep 2014 19:21:40 +0200 Subject: [PATCH] lidars: Add support for Sick Tim 551 and fix some identation issues --- src/lidars/hokuyo/hokuyo.h | 3 ++ src/lidars/lidar/lidar.h | 27 ++++++++------- src/lidars/lidar/lidar_params.h | 61 +++++++++++++++++++++------------ src/lidars/sick-tim3xx/sick.h | 7 ++-- src/types/robottype.ortegen | 45 ++++++++++++------------ 5 files changed, 86 insertions(+), 57 deletions(-) diff --git a/src/lidars/hokuyo/hokuyo.h b/src/lidars/hokuyo/hokuyo.h index fd99e27d..f6d32abb 100644 --- a/src/lidars/hokuyo/hokuyo.h +++ b/src/lidars/hokuyo/hokuyo.h @@ -4,6 +4,9 @@ #include #include +#define HOKUYO_RANGE_DIST_MIN_M 0.05 +#define HOKUYO_RANGE_DIST_MAX_M 4 + #define HOKUYO_ARRAY_SIZE (681) #define HOKUYO_SPLIT_DIVISION 1024 diff --git a/src/lidars/lidar/lidar.h b/src/lidars/lidar/lidar.h index 9dca688e..0cf53f77 100644 --- a/src/lidars/lidar/lidar.h +++ b/src/lidars/lidar/lidar.h @@ -2,21 +2,24 @@ #define LIDAR_H enum LIDAR_TYPE { - HOKUYO, - SICK_TIM3XX + HOKUYO, + SICK_TIM3XX, + SICK_TIM551 }; struct lidar_params { - enum LIDAR_TYPE type; - int data_lenght; // lenght of data - int split_div; // split division - int initial_meas; // initial measurement - int final_meas; // final measurement - double start_angle_deg; // start angle in degrees - int range_angle_left; - int range_angle_right; - int orientation; // see robodim.h - double center_offset_m; // see robodim.h. center offset in metres + enum LIDAR_TYPE type; + int data_lenght; // lenght of data + int split_div; // split division + int initial_meas; // initial measurement + int final_meas; // final measurement + double start_angle_deg; // start angle in degrees + int range_angle_left; + int range_angle_right; + int orientation; // see robodim.h + double center_offset_m; // see robodim.h. center offset in metres + double range_dist_min_m; + double range_dist_max_m; }; // C functions can be called from C++ code, e.g. RobomonAtlantis.cpp diff --git a/src/lidars/lidar/lidar_params.h b/src/lidars/lidar/lidar_params.h index b12d2e72..9403c56a 100644 --- a/src/lidars/lidar/lidar_params.h +++ b/src/lidars/lidar/lidar_params.h @@ -1,35 +1,54 @@ #ifndef LIDAR_PARAMS_H #define LIDAR_PARAMS_H +#include "lidar.h" #include #include #include -#include "lidar.h" const struct lidar_params hokuyo_params = { - HOKUYO, - HOKUYO_ARRAY_SIZE, - HOKUYO_SPLIT_DIVISION, - HOKUYO_INITIAL_MEASUREMENT, - HOKUYO_FINAL_MEASUREMENT, - HOKUYO_START_ANGLE, - HOKUYO_RANGE_ANGLE_LEFT, - HOKUYO_RANGE_ANGLE_RIGHT, - HOKUYO_ORIENTATION, - HOKUYO_CENTER_OFFSET_M + HOKUYO, + HOKUYO_ARRAY_SIZE, + HOKUYO_SPLIT_DIVISION, + HOKUYO_INITIAL_MEASUREMENT, + HOKUYO_FINAL_MEASUREMENT, + HOKUYO_START_ANGLE, + HOKUYO_RANGE_ANGLE_LEFT, + HOKUYO_RANGE_ANGLE_RIGHT, + HOKUYO_ORIENTATION, + HOKUYO_CENTER_OFFSET_M, + HOKUYO_RANGE_DIST_MIN_M, + HOKUYO_RANGE_DIST_MAX_M, }; const struct lidar_params sick_params = { - SICK_TIM3XX, - SICK_ARRAY_SIZE, - SICK_SPLIT_DIVISION, - SICK_INITIAL_MEASUREMENT, - SICK_FINAL_MEASUREMENT, - SICK_START_ANGLE, - SICK_RANGE_ANGLE_LEFT, - SICK_RANGE_ANGLE_RIGHT, - SICK_ORIENTATION, - SICK_CENTER_OFFSET_M + SICK_TIM3XX, + SICK_ARRAY_SIZE, + SICK_SPLIT_DIVISION, + SICK_INITIAL_MEASUREMENT, + SICK_FINAL_MEASUREMENT, + SICK_START_ANGLE, + SICK_RANGE_ANGLE_LEFT, + SICK_RANGE_ANGLE_RIGHT, + SICK_ORIENTATION, + SICK_CENTER_OFFSET_M, + SICK_RANGE_DIST_MIN_M, + SICK331_RANGE_DIST_MAX_M, +}; + +const struct lidar_params sick551_params = { + SICK_TIM551, + SICK_ARRAY_SIZE, + SICK_SPLIT_DIVISION, + SICK_INITIAL_MEASUREMENT, + SICK_FINAL_MEASUREMENT, + SICK_START_ANGLE, + SICK_RANGE_ANGLE_LEFT, + SICK_RANGE_ANGLE_RIGHT, + SICK_ORIENTATION, + SICK_CENTER_OFFSET_M, + SICK_RANGE_DIST_MIN_M, + SICK551_RANGE_DIST_MAX_M, }; #endif //LIDAR_PARAMS_H \ No newline at end of file diff --git a/src/lidars/sick-tim3xx/sick.h b/src/lidars/sick-tim3xx/sick.h index 55338c46..a6c95ff1 100644 --- a/src/lidars/sick-tim3xx/sick.h +++ b/src/lidars/sick-tim3xx/sick.h @@ -6,13 +6,16 @@ #define SICK_ARRAY_SIZE (271) +#define SICK_RANGE_DIST_MIN_M 0.05 +#define SICK331_RANGE_DIST_MAX_M 4 +#define SICK551_RANGE_DIST_MAX_M 10 #define SICK_SPLIT_DIVISION 360 #define SICK_START_ANGLE (270.00/2) // This value are not used for sick-tim3xx -#define SICK_FINAL_MEASUREMENT 0 -#define SICK_INITIAL_MEASUREMENT 0 +#define SICK_FINAL_MEASUREMENT 0 +#define SICK_INITIAL_MEASUREMENT 0 #define SICK_INDEX_TO_DEG(x) ((SICK_START_ANGLE-(x)*360.0/SICK_SPLIT_DIVISION) * SICK_ORIENTATION) #define SICK_INDEX_TO_RAD(x) (SICK_INDEX_TO_DEG(x)/180.0*M_PI) diff --git a/src/types/robottype.ortegen b/src/types/robottype.ortegen index c29aa18f..acafe309 100644 --- a/src/types/robottype.ortegen +++ b/src/types/robottype.ortegen @@ -2,27 +2,28 @@ type=cl_sensor_status topic=cl_sensor_status deadline=0.5 type=cl_sensor_cmd topic=cl_sensor_cmd -type=match_time topic=match_time deadline=0.5 -type=can_msg topic=can_msg -type=robot_pos topic=est_pos_odo -type=robot_pos topic=est_pos_indep_odo -type=robot_pos topic=est_pos_best +type=match_time topic=match_time deadline=0.5 +type=can_msg topic=can_msg +type=robot_pos topic=est_pos_odo +type=robot_pos topic=est_pos_indep_odo +type=robot_pos topic=est_pos_best type=lidar_scan topic=hokuyo_scan deadline=1 type=lidar_scan topic=sick_scan deadline=1 -type=odo_data topic=odo_data deadline=0.3 -type=motion_irc topic=motion_irc deadline=0.3 -type=motion_speed topic=motion_speed deadline=0.3 pubdelay=0.1 -type=motion_status topic=motion_status deadline=1.5 -type=pwr_alert topic=pwr_alert -type=pwr_ctrl topic=pwr_ctrl -type=pwr_voltage topic=pwr_voltage -type=robot_pos topic=ref_pos -type=robot_cmd topic=robot_cmd deadline=1 -type=robot_switches topic=robot_switches deadline=1 -type=robot_bumpers topic=robot_bumpers deadline=1 -type=camera_result topic=camera_result -type=camera_control topic=camera_control pubdelay=1 -type=fsm_state topic=fsm_main pubdelay=1 -type=fsm_state topic=fsm_act pubdelay=1 -type=fsm_state topic=fsm_motion pubdelay=1 -type=display_cmd topic=display_cmd +type=lidar_scan topic=sick551_scan deadline=1 +type=odo_data topic=odo_data deadline=0.3 +type=motion_irc topic=motion_irc deadline=0.3 +type=motion_speed topic=motion_speed deadline=0.3 pubdelay=0.1 +type=motion_status topic=motion_status deadline=1.5 +type=pwr_alert topic=pwr_alert +type=pwr_ctrl topic=pwr_ctrl +type=pwr_voltage topic=pwr_voltage +type=robot_pos topic=ref_pos +type=robot_cmd topic=robot_cmd deadline=1 +type=robot_switches topic=robot_switches deadline=1 +type=robot_bumpers topic=robot_bumpers deadline=1 +type=camera_result topic=camera_result +type=camera_control topic=camera_control pubdelay=1 +type=fsm_state topic=fsm_main pubdelay=1 +type=fsm_state topic=fsm_act pubdelay=1 +type=fsm_state topic=fsm_motion pubdelay=1 +type=display_cmd topic=display_cmd -- 2.39.2