MRPT  1.9.9
laser_scan.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <geometry_msgs/Pose.h>
13 #include <mrpt/ros1bridge/pose.h>
14 #include <mrpt/ros1bridge/time.h>
15 #include <mrpt/version.h>
16 #include <sensor_msgs/LaserScan.h>
17 
18 namespace mrpt::ros1bridge
19 {
20 bool convert(
21  const sensor_msgs::LaserScan& msg, const mrpt::poses::CPose3D& pose,
23 {
24  obj.timestamp = fromROS(msg.header.stamp);
25  obj.rightToLeft = true;
26  obj.sensorLabel = msg.header.frame_id;
27  obj.aperture = msg.angle_max - msg.angle_min;
28  obj.maxRange = msg.range_max;
29  obj.sensorPose = pose;
30 
31  ASSERT_(msg.ranges.size() > 1);
32 
33  const size_t N = msg.ranges.size();
34  const double ang_step = obj.aperture / (N - 1);
35  const double fov05 = 0.5 * obj.aperture;
36  const double inv_ang_step = (N - 1) / obj.aperture;
37 
38  obj.resizeScan(N);
39  for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
40  {
41  // ROS indices go from msg.angle_min to msg.angle_max, while
42  // in MRPT they go from -FOV/2 to +FOV/2.
43  int i_ros = inv_ang_step * (-fov05 - msg.angle_min + ang_step * i_mrpt);
44  if (i_ros < 0)
45  i_ros += N;
46  else if (i_ros >= (int)N)
47  i_ros -= N; // wrap around 2PI...
48 
49  // set the scan
50  const float r = msg.ranges[i_ros];
51  obj.setScanRange(i_mrpt, r);
52 
53  // set the validity of the scan
54  const auto ri = obj.getScanRange(i_mrpt);
55  const bool r_valid =
56  ((ri < (msg.range_max * 0.99)) && (ri > msg.range_min));
57  obj.setScanRangeValidity(i_mrpt, r_valid);
58  }
59 
60  return true;
61 }
62 
63 bool toROS(
64  const mrpt::obs::CObservation2DRangeScan& obj, sensor_msgs::LaserScan& msg)
65 {
66  const size_t nRays = obj.getScanSize();
67  if (!nRays) return false;
68 
69  msg.angle_min = -0.5f * obj.aperture;
70  msg.angle_max = 0.5f * obj.aperture;
71  msg.angle_increment = obj.aperture / (obj.getScanSize() - 1);
72 
73  // setting the following values to zero solves a rviz visualization problem
74  msg.time_increment = 0.0; // 1./30.; // Anything better?
75  msg.scan_time = 0.0; // msg.time_increment; // idem?
76 
77  msg.range_min = 0.02f;
78  msg.range_max = obj.maxRange;
79 
80  msg.ranges.resize(nRays);
81  for (size_t i = 0; i < nRays; i++) msg.ranges[i] = obj.getScanRange(i);
82 
83  // Set header data:
84  msg.header.stamp = toROS(obj.timestamp);
85  msg.header.frame_id = obj.sensorLabel;
86 
87  return true;
88 }
89 
90 bool toROS(
91  const mrpt::obs::CObservation2DRangeScan& obj, sensor_msgs::LaserScan& msg,
92  geometry_msgs::Pose& pose)
93 {
94  toROS(obj, msg);
95  pose = toROS_Pose(obj.sensorPose);
96  return true;
97 }
98 } // namespace mrpt::ros1bridge
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
bool convert(const sensor_msgs::LaserScan &msg, const mrpt::poses::CPose3D &pose, mrpt::obs::CObservation2DRangeScan &obj)
Definition: laser_scan.cpp:20
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
geometry_msgs::Pose toROS_Pose(const mrpt::poses::CPose2D &src)
Definition: pose.cpp:54



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019