laser_scan.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <geometry_msgs/Pose.h>
11 #include <sensor_msgs/LaserScan.h>
12 #include "mrpt_bridge/time.h"
13 #include "mrpt_bridge/pose.h"
14 #include "mrpt_bridge/laser_scan.h"
15 
16 #include <mrpt/version.h>
17 #include <mrpt/obs/CObservation2DRangeScan.h>
18 using namespace mrpt::obs;
19 
20 namespace mrpt_bridge
21 {
22 bool convert(
23  const sensor_msgs::LaserScan& _msg, const mrpt::poses::CPose3D& _pose,
24  CObservation2DRangeScan& _obj)
25 {
26  mrpt_bridge::convert(_msg.header.stamp, _obj.timestamp);
27  _obj.rightToLeft = true;
28  _obj.sensorLabel = _msg.header.frame_id;
29  _obj.aperture = _msg.angle_max - _msg.angle_min;
30  _obj.maxRange = _msg.range_max;
31  _obj.sensorPose = _pose;
32 
33  ASSERT_(_msg.ranges.size() > 1);
34 
35  const size_t N = _msg.ranges.size();
36  const double ang_step = _obj.aperture / (N - 1);
37  const double fov05 = 0.5 * _obj.aperture;
38  const double inv_ang_step = (N - 1) / _obj.aperture;
39 
40  _obj.resizeScan(N);
41  for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
42  {
43  // ROS indices go from _msg.angle_min to _msg.angle_max, while
44  // in MRPT they go from -FOV/2 to +FOV/2.
45  int i_ros =
46  inv_ang_step * (-fov05 - _msg.angle_min + ang_step * i_mrpt);
47  if (i_ros < 0)
48  i_ros += N;
49  else if (i_ros >= (int)N)
50  i_ros -= N; // wrap around 2PI...
51 
52  // set the scan
53  const float r = _msg.ranges[i_ros];
54  _obj.setScanRange(i_mrpt, r);
55 
56  // set the validity of the scan
57  const bool r_valid =
58  ((_obj.getScanRange(i_mrpt) < (_msg.range_max * 0.95)) &&
59  (_obj.getScanRange(i_mrpt) > _msg.range_min));
60  _obj.setScanRangeValidity(i_mrpt, r_valid);
61  }
62 
63  return true;
64 }
65 
66 bool convert(const CObservation2DRangeScan& _obj, sensor_msgs::LaserScan& _msg)
67 {
68  const size_t nRays = _obj.getScanSize();
69  if (!nRays) return false;
70 
71  _msg.angle_min = -0.5f * _obj.aperture;
72  _msg.angle_max = 0.5f * _obj.aperture;
73  _msg.angle_increment = _obj.aperture / (_obj.getScanSize() - 1);
74 
75  // setting the following values to zero solves a rviz visualization problem
76  _msg.time_increment = 0.0; // 1./30.; // Anything better?
77  _msg.scan_time = 0.0; // _msg.time_increment; // idem?
78 
79  _msg.range_min = 0.02;
80  _msg.range_max = _obj.maxRange;
81 
82  _msg.ranges.resize(nRays);
83  for (size_t i = 0; i < nRays; i++) _msg.ranges[i] = _obj.getScanRange(i);
84 
85  // Set header data:
86  mrpt_bridge::convert(_obj.timestamp, _msg.header.stamp);
87  _msg.header.frame_id = _obj.sensorLabel;
88 
89  return true;
90 }
91 
92 bool convert(
93  const CObservation2DRangeScan& _obj, sensor_msgs::LaserScan& _msg,
94  geometry_msgs::Pose& _pose)
95 {
96  convert(_obj, _msg);
97  mrpt::poses::CPose3D pose;
98  _obj.getSensorPose(pose);
99  convert(pose, _pose);
100  return true;
101 }
102 } // namespace mrpt_bridge
sensor_msgs::LaserScan_
Definition: laser_scan.h:31
mrpt_bridge::convert
bool convert(const CObservation2DRangeScan &_obj, sensor_msgs::LaserScan &_msg, geometry_msgs::Pose &_pose)
Definition: laser_scan.cpp:92
pose.h
mrpt::obs
Definition: include/mrpt_bridge/beacon.h:46
geometry_msgs::Pose_
Definition: include/mrpt_bridge/beacon.h:24
time.h
mrpt_bridge::convert
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types.
Definition: include/mrpt_bridge/beacon.h:52
laser_scan.h


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10