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_bridge/time.h"
00005 #include "mrpt_bridge/pose.h"
00006 #include "mrpt_bridge/laser_scan.h"
00007 
00008 #include <mrpt/version.h>
00009 #include <mrpt/obs/CObservation2DRangeScan.h>
00010 using namespace mrpt::obs;
00011 
00012 namespace mrpt_bridge
00013 {
00014 bool convert(
00015         const sensor_msgs::LaserScan& _msg, const mrpt::poses::CPose3D& _pose,
00016         CObservation2DRangeScan& _obj)
00017 {
00018         mrpt_bridge::convert(_msg.header.stamp, _obj.timestamp);
00019         _obj.rightToLeft = true;
00020         _obj.sensorLabel = _msg.header.frame_id;
00021         _obj.aperture = _msg.angle_max - _msg.angle_min;
00022         _obj.maxRange = _msg.range_max;
00023         _obj.sensorPose = _pose;
00024 
00025         ASSERT_(_msg.ranges.size() > 1);
00026 
00027         const size_t N = _msg.ranges.size();
00028         const double ang_step = _obj.aperture / (N - 1);
00029         const double fov05 = 0.5 * _obj.aperture;
00030         const double inv_ang_step = (N - 1) / _obj.aperture;
00031 
00032         _obj.resizeScan(N);
00033         for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
00034         {
00035                 // ROS indices go from _msg.angle_min to _msg.angle_max, while
00036                 // in MRPT they go from -FOV/2 to +FOV/2.
00037                 int i_ros =
00038                         inv_ang_step * (-fov05 - _msg.angle_min + ang_step * i_mrpt);
00039                 if (i_ros < 0)
00040                         i_ros += N;
00041                 else if (i_ros >= (int)N)
00042                         i_ros -= N;  // wrap around 2PI...
00043 
00044                 // set the scan
00045                 const float r = _msg.ranges[i_ros];
00046                 _obj.setScanRange(i_mrpt, r);
00047 
00048                 // set the validity of the scan
00049                 const bool r_valid =
00050                         ((_obj.scan[i_mrpt] < (_msg.range_max * 0.95)) &&
00051                          (_obj.scan[i_mrpt] > _msg.range_min));
00052                 _obj.setScanRangeValidity(i_mrpt, r_valid);
00053         }
00054 
00055         return true;
00056 }
00057 
00058 bool convert(const CObservation2DRangeScan& _obj, sensor_msgs::LaserScan& _msg)
00059 {
00060         const size_t nRays = _obj.scan.size();
00061         if (!nRays) return false;
00062 
00063         ASSERT_EQUAL_(_obj.scan.size(), _obj.validRange.size());
00064 
00065         _msg.angle_min = -0.5f * _obj.aperture;
00066         _msg.angle_max = 0.5f * _obj.aperture;
00067         _msg.angle_increment = _obj.aperture / (_obj.scan.size() - 1);
00068 
00069         // setting the following values to zero solves a rviz visualization problem
00070         _msg.time_increment = 0.0;  // 1./30.; // Anything better?
00071         _msg.scan_time = 0.0;  // _msg.time_increment; // idem?
00072 
00073         _msg.range_min = 0.02;
00074         _msg.range_max = _obj.maxRange;
00075 
00076         _msg.ranges.resize(nRays);
00077         for (size_t i = 0; i < nRays; i++) _msg.ranges[i] = _obj.scan[i];
00078 
00079         // Set header data:
00080         mrpt_bridge::convert(_obj.timestamp, _msg.header.stamp);
00081         _msg.header.frame_id = _obj.sensorLabel;
00082 
00083         return true;
00084 }
00085 
00086 bool convert(
00087         const CObservation2DRangeScan& _obj, sensor_msgs::LaserScan& _msg,
00088         geometry_msgs::Pose& _pose)
00089 {
00090         convert(_obj, _msg);
00091         mrpt::poses::CPose3D pose;
00092         _obj.getSensorPose(pose);
00093         convert(pose, _pose);
00094         return true;
00095 }
00096 }  // end namespace


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Apr 27 2018 05:10:54