apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #13 from mikeferguson/indigo-devel

Browse Source 
add pcl::PointCloud to sensor_msgs::Image converter
This commit is contained in:
Paul Bovbel 2014-05-27 20:02:37 -04:00
commit 3acde03e8a
1 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
include/pcl_conversions/pcl_conversions.h
View File

@ -2,6 +2,7 @@
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Open Source Robotics Foundation, Inc.
* Copyright (c) 2010-2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@ -502,6 +503,36 @@ namespace pcl {
pcl_conversions::moveFromPCL(pcl_image, image);
}
template<typename T> void
toROSMsg (const pcl::PointCloud<T> &cloud, sensor_msgs::Image& msg)
{
// Ease the user's burden on specifying width/height for unorganized datasets
if (cloud.width == 0 && cloud.height == 0)
{
throw std::runtime_error("Needs to be a dense like cloud!!");
}
else
{
if (cloud.points.size () != cloud.width * cloud.height)
throw std::runtime_error("The width and height do not match the cloud size!");
msg.height = cloud.height;
msg.width = cloud.width;
}
// sensor_msgs::image_encodings::BGR8;
msg.encoding = "bgr8";
msg.step = msg.width * sizeof (uint8_t) * 3;
msg.data.resize (msg.step * msg.height);
for (size_t y = 0; y < cloud.height; y++)
{
for (size_t x = 0; x < cloud.width; x++)
{
uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t));
}
}
}
/** Provide to/fromROSMsg for sensor_msgs::PointCloud2 <=> pcl::PointCloud<T> **/
template<typename T>