laser_scan.cpp
Go to the documentation of this file.
00001 
00002 #include <geometry_msgs/Pose.h>
00003 #include <sensor_msgs/LaserScan.h>
00004 #include <mrpt/slam/CObservation2DRangeScan.h>
00005 #include "mrpt_bridge/time.h"
00006 #include "mrpt_bridge/pose.h"
00007 #include "mrpt_bridge/laser_scan.h"
00008 
00009 namespace mrpt_bridge {
00010 
00011 bool convert(const sensor_msgs::LaserScan &_msg, const mrpt::poses::CPose3D &_pose, mrpt::slam::CObservation2DRangeScan  &_obj
00012                          )
00013 {
00014     mrpt_bridge::convert(_msg.header.stamp, _obj.timestamp);
00015     _obj.rightToLeft = true;
00016     _obj.sensorLabel = _msg.header.frame_id;
00017     _obj.aperture = _msg.angle_max - _msg.angle_min;
00018     _obj.maxRange = _msg.range_max;
00019     _obj.sensorPose =  _pose;
00020     _obj.validRange.resize(_msg.ranges.size());
00021     _obj.scan.resize(_msg.ranges.size());
00022     for(std::size_t i = 0; i < _msg.ranges.size(); i++) {
00023         _obj.scan[i] = _msg.ranges[i];
00024         if((_obj.scan[i] < (_msg.range_max*0.95)) && (_obj.scan[i] > _msg.range_min)) {
00025             _obj.validRange[i] = 1;
00026         } else {
00027             _obj.validRange[i] = 0;
00028         }
00029     }
00030 
00031     return true;
00032 }
00033 
00034 bool convert(const mrpt::slam::CObservation2DRangeScan  &_obj, sensor_msgs::LaserScan &_msg) {
00035     const size_t nRays = _obj.scan.size();
00036     if (!nRays) return false;
00037 
00038     ASSERT_EQUAL_(_obj.scan.size(),_obj.validRange.size() )
00039 
00040     _msg.angle_min = -0.5f * _obj.aperture;
00041     _msg.angle_max =  0.5f * _obj.aperture;
00042     _msg.angle_increment = _obj.aperture/(_obj.scan.size()-1);
00043 
00044     // setting the following values to zero solves a rviz visualization problem
00045     _msg.time_increment = 0.0; // 1./30.; // Anything better?
00046     _msg.scan_time = 0.0; // _msg.time_increment; // idem?
00047 
00048     _msg.range_min = 0.02;
00049     _msg.range_max = _obj.maxRange;
00050 
00051     _msg.ranges.resize(nRays);
00052     for (size_t i=0; i<nRays; i++)
00053         _msg.ranges[i] = _obj.scan[i];
00054 
00055     // Set header data:
00056     mrpt_bridge::convert(_obj.timestamp, _msg.header.stamp);
00057     _msg.header.frame_id = _obj.sensorLabel;
00058 
00059     return true;
00060 }
00061 
00062 bool convert(const mrpt::slam::CObservation2DRangeScan  &_obj, sensor_msgs::LaserScan &_msg, geometry_msgs::Pose &_pose) {
00063     convert(_obj, _msg);
00064     mrpt::poses::CPose3D pose;
00065     _obj.getSensorPose(pose);
00066     convert(pose,_pose);
00067 }
00068 } // end namespace


mrpt_bridge
Author(s):
autogenerated on Tue Aug 5 2014 10:58:06