Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: conversions.h 3180 2011-11-17 16:25:52Z mdixon $ 00035 * 00036 */ 00037 00038 #ifndef PCL_ROS_CONVERSIONS_H_ 00039 #define PCL_ROS_CONVERSIONS_H_ 00040 00041 #include <sensor_msgs/PointField.h> 00042 #include <sensor_msgs/PointCloud2.h> 00043 #include <sensor_msgs/Image.h> 00044 #include "pcl/point_cloud.h" 00045 #include "pcl/ros/point_traits.h" 00046 #include "pcl/ros/for_each_type.h" 00047 #include "pcl/exceptions.h" 00048 #include <boost/foreach.hpp> 00049 00050 namespace pcl 00051 { 00052 namespace detail 00053 { 00054 // For converting template point cloud to message. 00055 template<typename PointT> 00056 struct FieldAdder 00057 { 00058 FieldAdder (std::vector<sensor_msgs::PointField>& fields) : fields_ (fields) {}; 00059 00060 template<typename U> void operator() () 00061 { 00062 sensor_msgs::PointField f; 00063 f.name = traits::name<PointT, U>::value; 00064 f.offset = traits::offset<PointT, U>::value; 00065 f.datatype = traits::datatype<PointT, U>::value; 00066 f.count = traits::datatype<PointT, U>::size; 00067 fields_.push_back (f); 00068 } 00069 00070 std::vector<sensor_msgs::PointField>& fields_; 00071 }; 00072 00073 // For converting message to template point cloud. 00074 template<typename PointT> 00075 struct FieldMapper 00076 { 00077 FieldMapper (const std::vector<sensor_msgs::PointField>& fields, 00078 std::vector<FieldMapping>& map) 00079 : fields_ (fields), map_ (map) 00080 { 00081 } 00082 00083 template<typename Tag> void operator() () 00084 { 00085 const char* name = traits::name<PointT, Tag>::value; 00086 BOOST_FOREACH(const sensor_msgs::PointField& field, fields_) 00087 { 00088 if (field.name == name) 00089 { 00090 typedef traits::datatype<PointT, Tag> Data; 00091 assert (Data::value == field.datatype); 00092 //@todo: Talk to Patrick about this 00093 //assert (Data::size == field.count); 00094 00095 FieldMapping mapping; 00096 mapping.serialized_offset = field.offset; 00097 mapping.struct_offset = traits::offset<PointT, Tag>::value; 00098 mapping.size = sizeof (typename Data::type); 00099 map_.push_back (mapping); 00100 return; 00101 } 00102 } 00103 // didn't find it... 00104 std::stringstream ss; 00105 ss << "Failed to find a field named: '" << name 00106 << "'. Cannot convert message to PCL type."; 00107 PCL_ERROR ("%s\n", ss.str().c_str()); 00108 throw pcl::InvalidConversionException(ss.str()); 00109 } 00110 00111 const std::vector<sensor_msgs::PointField>& fields_; 00112 std::vector<FieldMapping>& map_; 00113 }; 00114 00115 inline bool fieldOrdering(const FieldMapping& a, const FieldMapping& b) 00116 { 00117 return a.serialized_offset < b.serialized_offset; 00118 } 00119 00120 } //namespace detail 00121 00122 template<typename PointT> 00123 void createMapping (const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map) 00124 { 00125 // Create initial 1-1 mapping between serialized data segments and struct fields 00126 detail::FieldMapper<PointT> mapper (msg_fields, field_map); 00127 for_each_type< typename traits::fieldList<PointT>::type > (mapper); 00128 00129 // Coalesce adjacent fields into single memcpy's where possible 00130 if (field_map.size() > 1) 00131 { 00132 std::sort(field_map.begin(), field_map.end(), detail::fieldOrdering); 00133 MsgFieldMap::iterator i = field_map.begin(), j = i + 1; 00134 while (j != field_map.end()) 00135 { 00136 // This check is designed to permit padding between adjacent fields. 00139 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset) 00140 { 00141 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size); 00142 j = field_map.erase(j); 00143 } 00144 else 00145 { 00146 ++i; 00147 ++j; 00148 } 00149 } 00150 } 00151 } 00152 00153 template<typename PointT> 00154 void fromROSMsg (const sensor_msgs::PointCloud2& msg, pcl::PointCloud<PointT>& cloud, 00155 const MsgFieldMap& field_map) 00156 { 00157 // Copy info fields 00158 cloud.header = msg.header; 00159 cloud.width = msg.width; 00160 cloud.height = msg.height; 00161 cloud.is_dense = msg.is_dense == 1; 00162 00163 // Copy point data 00164 uint32_t num_points = msg.width * msg.height; 00165 cloud.points.resize (num_points); 00166 uint8_t* cloud_data = reinterpret_cast<uint8_t*>(&cloud.points[0]); 00167 00168 // Check if we can copy adjacent points in a single memcpy 00169 if (field_map.size() == 1 && 00170 field_map[0].serialized_offset == 0 && 00171 field_map[0].struct_offset == 0 && 00172 msg.point_step == sizeof(PointT)) 00173 { 00174 uint32_t cloud_row_step = sizeof(PointT) * cloud.width; 00175 const uint8_t* msg_data = &msg.data[0]; 00176 // Should usually be able to copy all rows at once 00177 if (msg.row_step == cloud_row_step) 00178 { 00179 memcpy (cloud_data, msg_data, msg.data.size()); 00180 } 00181 else 00182 { 00183 for (uint32_t i = 0; i < msg.height; 00184 ++i, cloud_data += cloud_row_step, msg_data += msg.row_step) 00185 memcpy (cloud_data, msg_data, cloud_row_step); 00186 } 00187 } 00188 else 00189 { 00190 // If not, memcpy each group of contiguous fields separately 00191 for (uint32_t row = 0; row < msg.height; ++row) 00192 { 00193 const uint8_t* row_data = &msg.data[row * msg.row_step]; 00194 for (uint32_t col = 0; col < msg.width; ++col) 00195 { 00196 const uint8_t* msg_data = row_data + col * msg.point_step; 00197 BOOST_FOREACH (const detail::FieldMapping& mapping, field_map) 00198 { 00199 memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); 00200 } 00201 cloud_data += sizeof (PointT); 00202 } 00203 } 00204 } 00205 } 00206 00207 template<typename PointT> 00208 void fromROSMsg (const sensor_msgs::PointCloud2& msg, pcl::PointCloud<PointT>& cloud) 00209 { 00210 MsgFieldMap field_map; 00211 createMapping<PointT> (msg.fields, field_map); 00212 fromROSMsg (msg, cloud, field_map); 00213 } 00214 00215 template<typename PointT> 00216 void toROSMsg (const pcl::PointCloud<PointT>& cloud, sensor_msgs::PointCloud2& msg) 00217 { 00218 // Ease the user's burden on specifying width/height for unorganized datasets 00219 if (cloud.width == 0 && cloud.height == 0) 00220 { 00221 msg.width = (uint32_t) cloud.points.size (); 00222 msg.height = 1; 00223 } 00224 else 00225 { 00226 assert (cloud.points.size () == cloud.width * cloud.height); 00227 msg.height = cloud.height; 00228 msg.width = cloud.width; 00229 } 00230 00231 // Fill point cloud binary data (padding and all) 00232 size_t data_size = sizeof (PointT) * cloud.points.size (); 00233 msg.data.resize (data_size); 00234 memcpy (&msg.data[0], &cloud.points[0], data_size); 00235 00236 // Fill fields metadata 00237 msg.fields.clear (); 00238 for_each_type< typename traits::fieldList<PointT>::type > (detail::FieldAdder<PointT>(msg.fields)); 00239 00240 msg.header = cloud.header; 00241 msg.point_step = sizeof (PointT); 00242 msg.row_step = sizeof (PointT) * msg.width; 00243 msg.is_dense = cloud.is_dense; 00245 } 00246 00253 template<typename CloudT> void 00254 toROSMsg(const CloudT& cloud, sensor_msgs::Image& msg) 00255 { 00256 // Ease the user's burden on specifying width/height for unorganized datasets 00257 if (cloud.width == 0 && cloud.height == 0) 00258 throw std::runtime_error("Needs to be a dense like cloud!!"); 00259 else 00260 { 00261 if(cloud.points.size () != cloud.width * cloud.height){ 00262 throw std::runtime_error("The width and height do not match the cloud size!"); 00263 } 00264 msg.height = cloud.height; 00265 msg.width = cloud.width; 00266 } 00267 00268 // ensor_msgs::image_encodings::BGR8; 00269 msg.encoding = "bgr8"; 00270 msg.step = msg.width * sizeof(uint8_t) * 3; 00271 msg.data.resize(msg.step * msg.height); 00272 for (size_t y = 0; y < cloud.height; y++) 00273 { 00274 for (size_t x = 0; x < cloud.width; x++) 00275 { 00276 uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); 00277 memcpy(pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t)); 00278 } 00279 } 00280 } 00281 00287 inline void 00288 toROSMsg (const sensor_msgs::PointCloud2& cloud, sensor_msgs::Image& msg) 00289 { 00290 int rgb_index = -1; 00291 // Get the index we need 00292 for (size_t d = 0; d < cloud.fields.size (); ++d) 00293 if (cloud.fields[d].name == "rgb") 00294 { 00295 rgb_index = (int) d; 00296 break; 00297 } 00298 00299 if(rgb_index == -1) 00300 throw std::runtime_error ("No rgb field!!"); 00301 if (cloud.width == 0 && cloud.height == 0) 00302 throw std::runtime_error ("Needs to be a dense like cloud!!"); 00303 else 00304 { 00305 msg.height = cloud.height; 00306 msg.width = cloud.width; 00307 } 00308 int rgb_offset = cloud.fields[rgb_index].offset; 00309 int point_step = cloud.point_step; 00310 00311 // sensor_msgs::image_encodings::BGR8; 00312 msg.encoding = "bgr8"; 00313 msg.step = msg.width * sizeof(uint8_t) * 3; 00314 msg.data.resize (msg.step * msg.height); 00315 00316 for (size_t y = 0; y < cloud.height; y++) 00317 { 00318 for (size_t x = 0; x < cloud.width; x++, rgb_offset += point_step) 00319 { 00320 uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); 00321 memcpy (pixel, &(cloud.data[rgb_offset]), 3 * sizeof (uint8_t)); 00322 } 00323 } 00324 } 00325 } 00326 00327 #endif //#ifndef PCL_ROS_CONVERSIONS_H_