point_cloud.cpp
Go to the documentation of this file.
00001 
00002 #include "mrpt_bridge/point_cloud.h"
00003 #include <ros/console.h>
00004 
00005 namespace mrpt_bridge
00006 {
00011 bool point_cloud::ros2mrpt(
00012         const sensor_msgs::PointCloud& msg, CSimplePointsMap& obj)
00013 {
00014         const size_t N = msg.points.size();
00015 
00016         obj.clear();
00017         obj.reserve(N);
00018         for (size_t i = 0; i < N; i++)
00019                 obj.insertPoint(msg.points[i].x, msg.points[i].y, msg.points[i].z);
00020 
00021         return true;
00022 }
00023 
00032 bool point_cloud::mrpt2ros(
00033         const CSimplePointsMap& obj, const std_msgs::Header& msg_header,
00034         sensor_msgs::PointCloud& msg)
00035 {
00036         // 1) sensor_msgs::PointCloud:: header
00037         msg.header = msg_header;
00038 
00039         // 2) sensor_msgs::PointCloud:: points
00040         const size_t N = obj.size();
00041         msg.points.resize(N);
00042         for (size_t i = 0; i < N; i++)
00043         {
00044                 geometry_msgs::Point32& pt = msg.points[i];
00045                 obj.getPoint(i, pt.x, pt.y, pt.z);
00046         }
00047 
00048         // 3) sensor_msgs::PointCloud:: channels
00049         msg.channels.clear();
00050 
00051         return true;
00052 }
00053 
00054 }  // namespace mrpt_bridge


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