00001 #ifndef __LASER_SCAN_OBSERVER_H 00002 #define __LASER_SCAN_OBSERVER_H 00003 00004 #include <utility> 00005 #include <sensor_msgs/LaserScan.h> 00006 #include <boost/shared_ptr.hpp> 00007 00008 #include "../core/sensor_data.h" 00009 #include "topic_with_transform.h" 00010 00015 class LaserScanObserver : public TopicObserver<sensor_msgs::LaserScan> { 00016 using ScanPtr = boost::shared_ptr<sensor_msgs::LaserScan>; 00017 public: //methods 00022 LaserScanObserver(bool skip_max_vals = false): 00023 _skip_max_vals(skip_max_vals), 00024 _prev_x(0), _prev_y(0), _prev_yaw(0) {} 00031 virtual void handle_transformed_msg( 00032 const ScanPtr msg, const tf::StampedTransform& t) { 00033 00034 double new_x = t.getOrigin().getX(); 00035 double new_y = t.getOrigin().getY(); 00036 double new_yaw = tf::getYaw(t.getRotation()); 00037 00038 TransformedLaserScan laser_scan; 00039 laser_scan.quality = 1.0; 00040 double angle = msg->angle_min; 00041 00042 for (const auto &range : msg->ranges) { 00043 ScanPoint sp(range, angle); 00044 angle += msg->angle_increment; 00045 00046 if (sp.range < msg->range_min) { 00047 continue; 00048 } else if (msg->range_max <= sp.range) { 00049 sp.is_occupied = false; 00050 sp.range = msg->range_max; 00051 if (_skip_max_vals) { 00052 continue; 00053 } 00054 } 00055 laser_scan.points.push_back(sp); 00056 } 00057 00058 laser_scan.d_x = new_x - _prev_x; 00059 laser_scan.d_y = new_y - _prev_y; 00060 laser_scan.d_yaw = new_yaw - _prev_yaw; 00061 _prev_x = new_x, _prev_y = new_y, _prev_yaw = new_yaw; 00062 00063 handle_laser_scan(laser_scan); 00064 } 00065 00066 virtual void handle_laser_scan(TransformedLaserScan &) = 0; 00067 00068 private: // fields 00069 bool _skip_max_vals; 00070 double _prev_x, _prev_y, _prev_yaw; 00071 }; 00072 00073 #endif