ros-perception / perception_pcl

PCL (Point Cloud Library) ROS interface stack
http://wiki.ros.org/perception_pcl
426 stars 334 forks source link

Serializer<pcl::PointCloud<T>>::read(stream,m) hangs #40

Open astaa-nrec opened 11 years ago

astaa-nrec commented 11 years ago

Consider the program:

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

void
cloud_cb (const pcl::PointCloud<pcl::PointXY>::ConstPtr& input)
{
  ROS_INFO("received a point cloud");
}

int
main (int argc, char** argv)
{
  ros::init (argc, argv, "cloud_listener");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<pcl::PointCloud<pcl::PointXY> > ("cloud", 1, cloud_cb);
  ros::spin ();
}

We can get this program to freeze by sending the message:

$ rostopic pub /cloud sensor_msgs/PointCloud2 "{'width':0xffffffff, 'height':0xffffffff}" -1

The program gets stuck in this loop in pcl_ros/point_cloud.h:

          // If not, do a lot of memcpys to copy over the fields
          for (uint32_t row = 0; row < m.height; ++row) {
            const uint8_t* stream_data = stream.advance(row_step);
            for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
              BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
                memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
              }
              m_data += sizeof(T);
            }
          }

Two different ways this could be fixed:

1) Add an assertion like:

assert(data_size > 0 || (m.width == 0 && m.height == 0));

This says that if the message has no data content, then its width and height fields are zero. This is better than just assert(data_size > 0) because we don't want default empty messages to crash the node.

2) Add a conditional like:

if (data_size > 0) { ...

so that no memcpy loop is attempted if it is known that the message has no data content. This would have the advantage of not crashing the node when such a message arrives.


(edited: data_size is probably the right variable to check, not row_step)

wjwwood commented 11 years ago

Thanks for the clear issue report. I'll look into making a fix for this when I get a chance, but this will get addressed faster if you can provide a pull request. I'll report back here when I start working on a fix (as not to have us both working on it).