MRPT  2.0.4
point_cloud.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-2020, 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 
11 #include <ros/console.h>
12 
13 using namespace mrpt::maps;
14 
15 namespace mrpt::ros1bridge
16 {
17 bool fromROS(const sensor_msgs::PointCloud& msg, CSimplePointsMap& obj)
18 {
19  const size_t N = msg.points.size();
20 
21  obj.clear();
22  obj.reserve(N);
23  for (size_t i = 0; i < N; i++)
24  obj.insertPoint(msg.points[i].x, msg.points[i].y, msg.points[i].z);
25 
26  return true;
27 }
28 
29 bool toROS(
30  const CSimplePointsMap& obj, const std_msgs::Header& msg_header,
31  sensor_msgs::PointCloud& msg)
32 {
33  // 1) sensor_msgs::PointCloud:: header
34  msg.header = msg_header;
35 
36  // 2) sensor_msgs::PointCloud:: points
37  const size_t N = obj.size();
38  msg.points.resize(N);
39  for (size_t i = 0; i < N; i++)
40  {
41  geometry_msgs::Point32& pt = msg.points[i];
42  obj.getPoint(i, pt.x, pt.y, pt.z);
43  }
44 
45  // 3) sensor_msgs::PointCloud:: channels
46  msg.channels.clear();
47 
48  return true;
49 }
50 
51 } // namespace mrpt::ros1bridge
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:198
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:658
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
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:459



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020