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
00036
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;
00043
00044
00045 const float r = _msg.ranges[i_ros];
00046 _obj.setScanRange(i_mrpt, r);
00047
00048
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
00070 _msg.time_increment = 0.0;
00071 _msg.scan_time = 0.0;
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
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 }