sicktoolbox_wrapper: sicktoolbox_wrapper: sickld.cpp Source File

00001 /*
00002  * Authors: Nick Hillier and Fred Pauling (CSIRO, 2011)
00003  * 
00004  * Based on the sicklms.cpp from the sicktoolbox_wrapper ROS package
00005  * and the sample code from the sicktoolbox manual.
00006  * 
00007  * Released under BSD license.
00008  */ 
00009 
00010 #include <iostream>
00011 #include <sickld-1.0/SickLD.hh>
00012 #include "ros/ros.h"
00013 #include "sensor_msgs/LaserScan.h"
00014 #include <deque>
00015 
00016 #define DEG2RAD(x) ((x)*M_PI/180.)
00017 
00018 using namespace std;
00019 using namespace SickToolbox;
00020 
00021 // TODO: refactor these functions into a common util lib (similar to code in sicklms.cpp)
00022 void publish_scan(ros::Publisher *pub, double *range_values,
00023                   uint32_t n_range_values, unsigned int *intensity_values,
00024                   uint32_t n_intensity_values, ros::Time start,
00025                   double scan_time, bool inverted, float angle_min,
00026                   float angle_max, std::string frame_id)
00027 {
00028   static int scan_count = 0;
00029   sensor_msgs::LaserScan scan_msg;
00030   scan_msg.header.frame_id = frame_id;
00031   scan_count++;
00032   if (inverted) { // assumes scan window at the bottom
00033     scan_msg.angle_min = angle_max;
00034     scan_msg.angle_max = angle_min;
00035   } else {
00036     scan_msg.angle_min = angle_min;
00037     scan_msg.angle_max = angle_max;
00038   }
00039   scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) / (double)(n_range_values-1);
00040   scan_msg.scan_time = scan_time;
00041   scan_msg.time_increment = scan_time / n_range_values;
00042   scan_msg.range_min = 0.5;
00043   scan_msg.range_max = 250.;
00044   scan_msg.ranges.resize(n_range_values);
00045   scan_msg.header.stamp = start;
00046   for (size_t i = 0; i < n_range_values; i++) {
00047     scan_msg.ranges[i] = (float)range_values[i];
00048   }
00049   scan_msg.intensities.resize(n_intensity_values);
00050   for (size_t i = 0; i < n_intensity_values; i++) {
00051     scan_msg.intensities[i] = (float)intensity_values[i];
00052   }
00053   pub->publish(scan_msg);
00054 }
00055 
00056 // A complimentary filter to get a (much) better time estimate, does not
00057 // calibrate out constant network latency delays, but does get rid of 
00058 // timming jitter to get better timing estimates than the 
00059 // communicated clock resolution (which is only 1ms)
00060 class smoothtime { 
00061     protected:
00062         ros::Time smoothtime_prev, smoothed_timestamp;
00063         double time_smoothing_factor;
00064         double error_threshold;
00065     public:
00067         smoothtime(){
00068             time_smoothing_factor = 0.95; 
00069             error_threshold = .50; 
00070         }
00072         void set_smoothing_factor(double smoothing_factor){ 
00073             time_smoothing_factor = smoothing_factor;
00074         }
00076         void set_error_threshold(double err_threshold){ 
00077             error_threshold = err_threshold;
00078         }
00079         ros::Time smooth_timestamp(ros::Time recv_timestamp, ros::Duration expctd_dur) {
00080             if (smoothtime_prev.is_zero() == true) {
00081                 smoothed_timestamp = recv_timestamp;
00082             } else {
00083                 smoothed_timestamp = smoothtime_prev + expctd_dur;
00084                 double err = (recv_timestamp - smoothed_timestamp).toSec();
00085                 double time_error_threshold = expctd_dur.toSec() * error_threshold;
00086                 if ((time_smoothing_factor > 0) && (fabs(err) < time_error_threshold)){
00087                     ros::Duration correction = ros::Duration(err * (1 - time_smoothing_factor));
00088                     smoothed_timestamp += correction;
00089                 } else {
00090                     // error too high, or smoothing disabled - set smoothtime to last timestamp
00091                     smoothed_timestamp = recv_timestamp;
00092                 }
00093             }
00094             smoothtime_prev = smoothed_timestamp;
00095             return smoothed_timestamp;
00096         }
00097 };
00098 
00099 class averager {
00100     protected:
00101         std::deque<double> deq;
00102         unsigned int max_len;
00103         double sum;
00104     public:
00105         averager(int max_len = 50){
00106             this->max_len = max_len;
00107         }
00108         void add_new(double data) {
00109             deq.push_back(data);
00110             sum += data;
00111             if (deq.size() > max_len) {
00112                 sum -= deq.front();
00113                 deq.pop_front();
00114             }
00115         }
00116         double get_mean() {
00117             return sum/deq.size();
00118         }
00119 };
00120         
00121 
00122 int main(int argc, char *argv[]) {
00123     ros::init(argc, argv, "sickld");
00124     int port;
00125     std::string ipaddress;
00126     std::string frame_id;
00127     bool inverted;
00128     int sick_motor_speed = 5;//10; // Hz
00129     double sick_step_angle = 1.5;//0.5;//0.25; // deg (0.125 = no gaps between spots)
00130     double active_sector_start_angle = 0;
00131     double active_sector_stop_angle = 300;//269.75;
00132     double smoothing_factor, error_threshold;
00133     ros::NodeHandle nh;
00134         ros::NodeHandle nh_ns("~");
00135         ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
00136         nh_ns.param("port", port, DEFAULT_SICK_TCP_PORT);
00137         nh_ns.param("ipaddress", ipaddress, (std::string)DEFAULT_SICK_IP_ADDRESS);
00138         nh_ns.param("inverted", inverted, false);
00139         nh_ns.param<std::string>("frame_id", frame_id, "laser");
00140     nh_ns.param("timer_smoothing_factor", smoothing_factor, 0.97);
00141     nh_ns.param("timer_error_threshold", error_threshold, 0.5);
00142     nh_ns.param("resolution", sick_step_angle, 1.0); 
00143     nh_ns.param("start_angle", active_sector_start_angle, 0.); 
00144     nh_ns.param("stop_angle", active_sector_stop_angle, 300.); 
00145     nh_ns.param("scan_rate", sick_motor_speed, 10); 
00146     /* Define buffers for return values */
00147     double range_values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0};
00148     unsigned int intensity_values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0};
00149     /* Define buffers to hold sector specific data */
00150     unsigned int num_measurements = {0};
00151     unsigned int sector_start_timestamp = {0};
00152         unsigned int sector_stop_timestamp = {0};
00153     double sector_step_angle = {0};
00154     double sector_start_angle = {0};
00155     double sector_stop_angle = {0};
00156     /* Instantiate the object */
00157     SickLD sick_ld(ipaddress.c_str(),port);
00158     try {
00159         /* Initialize the device */
00160         sick_ld.Initialize();
00161         // TODO: do some calls to setup the device - e.g. scan rate.
00162         try {
00163             sick_ld.SetSickGlobalParamsAndScanAreas((unsigned int)sick_motor_speed,
00164                                           sick_step_angle,
00165                                           &active_sector_start_angle,
00166                                           &active_sector_stop_angle,
00167                                           (unsigned int)1);
00168         } catch (...) {
00169             ROS_ERROR("Configuration error");
00170             return -1;
00171         }
00172         smoothtime smoothtimer;
00173         averager avg_fulldur, avg_scandur;
00174         smoothtimer.set_smoothing_factor(smoothing_factor);
00175         smoothtimer.set_error_threshold(error_threshold);
00176         ros::Time last_start_scan_time;
00177         unsigned int last_sector_stop_timestamp = 0;
00178         double full_duration;
00179         while (ros::ok()) {
00180             /* Grab the measurements (from all sectors) */
00181             sick_ld.GetSickMeasurements(range_values,
00182                                         intensity_values,
00183                                         &num_measurements,
00184                                         NULL,
00185                                         NULL,
00186                                         &sector_step_angle,
00187                                         &sector_start_angle,
00188                                         &sector_stop_angle,
00189                                         &sector_start_timestamp,
00190                                         &sector_stop_timestamp
00191                                         );
00192             ros::Time end_scan_time = ros::Time::now();
00193             
00194             double scan_duration = (sector_stop_timestamp - sector_start_timestamp) * 1e-3;
00195             avg_scandur.add_new(scan_duration);
00196             scan_duration = avg_scandur.get_mean();
00197             
00198             if (last_sector_stop_timestamp == 0) {
00199                 full_duration = 1./((double)sick_motor_speed);
00200             } else {
00201                 full_duration = (sector_stop_timestamp - last_sector_stop_timestamp) * 1e-3;
00202             }
00203             avg_fulldur.add_new(full_duration);
00204             full_duration = avg_fulldur.get_mean();
00205             
00206             ros::Time smoothed_end_scan_time = smoothtimer.smooth_timestamp(end_scan_time, ros::Duration(full_duration));
00207             ros::Time start_scan_time = smoothed_end_scan_time - ros::Duration(scan_duration);
00208             
00209             publish_scan(&scan_pub, range_values, num_measurements, intensity_values,
00210                    num_measurements, start_scan_time, scan_duration, inverted,
00211                    DEG2RAD((float)sector_start_angle), DEG2RAD((float)sector_stop_angle), frame_id);
00212 
00213             ROS_DEBUG_STREAM("Num meas: " << num_measurements
00214                  << " smoothed start T: " << start_scan_time
00215                  << " smoothed rate: " << 1./(start_scan_time - last_start_scan_time).toSec()
00216                  << " raw start T: " << sector_start_timestamp
00217                  << " raw stop T: " << sector_stop_timestamp
00218                  << " dur: " << full_duration
00219                  << " step A: " << sector_step_angle
00220                  << " start A: " << sector_start_angle
00221                  << " stop A: " << sector_stop_angle);
00222             last_start_scan_time = start_scan_time;
00223             last_sector_stop_timestamp = sector_stop_timestamp;
00224                         ros::spinOnce();
00225         }
00226         /* Uninitialize the device */
00227         sick_ld.Uninitialize();
00228     }
00229     catch(...) {
00230         ROS_ERROR("Error");
00231         return -1;
00232     }
00233     return 0;
00234 }