From 1a8e94ddbed8bc6881d68b9d987d152c569e446a Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 20 Jan 2015 10:14:42 -0500 Subject: [PATCH] Fix regression with pointcloud2 iter double->float --- .../src/pointcloud_to_laserscan_nodelet.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 81dd7eb9..5bd567c4 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -127,14 +127,14 @@ namespace pointcloud_to_laserscan void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) { - if(target_frame_.empty()){ - target_frame_ = cloud_msg->header.frame_id; - } //build laserscan output sensor_msgs::LaserScan output; output.header = cloud_msg->header; - output.header.frame_id = target_frame_; + if(!target_frame_.empty()){ + output.header.frame_id = target_frame_; + } + output.angle_min = angle_min_; output.angle_max = angle_max_; output.angle_increment = angle_increment_; @@ -175,9 +175,9 @@ namespace pointcloud_to_laserscan } // Iterate through pointcloud - sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_out, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_out, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_out, "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_out, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_out, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_out, "z"); for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ @@ -217,4 +217,3 @@ namespace pointcloud_to_laserscan } PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); -