Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef MCL_3DL_POINT_CONVERSION_H
00031 #define MCL_3DL_POINT_CONVERSION_H
00032
00033 #include <ros/ros.h>
00034
00035 #include <mcl_3dl/point_types.h>
00036
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl_ros/point_cloud.h>
00040
00041 #include <sensor_msgs/PointCloud2.h>
00042 #include <sensor_msgs/point_cloud_conversion.h>
00043
00044 namespace mcl_3dl
00045 {
00046 namespace
00047 {
00048 template <typename PointTIn, typename PointTOut>
00049 bool fromROSMsgImpl(
00050 const sensor_msgs::PointCloud2& msg, pcl::PointCloud<PointTOut>& pc)
00051 {
00052 typename pcl::PointCloud<PointTIn>::Ptr raw(new typename pcl::PointCloud<PointTIn>);
00053 pcl::fromROSMsg(msg, *raw);
00054 if (raw->points.size() == 0)
00055 {
00056 ROS_ERROR("Given PointCloud2 is empty");
00057 return false;
00058 }
00059 pcl::copyPointCloud(*raw, pc);
00060 return true;
00061 }
00062 }
00063
00064 template <typename PointT>
00065 bool fromROSMsg(
00066 const sensor_msgs::PointCloud2& msg, pcl::PointCloud<PointT>& pc)
00067 {
00068 const int x_idx = getPointCloud2FieldIndex(msg, "x");
00069 const int y_idx = getPointCloud2FieldIndex(msg, "y");
00070 const int z_idx = getPointCloud2FieldIndex(msg, "z");
00071 const int intensity_idx = getPointCloud2FieldIndex(msg, "intensity");
00072
00073 if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00074 {
00075 ROS_ERROR("Given PointCloud2 doesn't have x, y, z fields");
00076 return false;
00077 }
00078 if (intensity_idx != -1)
00079 return fromROSMsgImpl<pcl::PointXYZI, PointT>(msg, pc);
00080
00081 return fromROSMsgImpl<pcl::PointXYZ, PointT>(msg, pc);
00082 }
00083 }
00084 #endif // MCL_3DL_POINT_CONVERSION_H