diff --git a/rslidar_pointcloud/src/convert.cc b/rslidar_pointcloud/src/convert.cc index c56ccf7..ee20453 100644 --- a/rslidar_pointcloud/src/convert.cc +++ b/rslidar_pointcloud/src/convert.cc @@ -41,6 +41,7 @@ Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) : data_(new r private_nh.param("input_packets_topic", input_packets_topic, std::string("rslidar_packets")); rslidar_scan_ = node.subscribe(input_packets_topic, 10, &Convert::processScan, (Convert*)this, ros::TransportHints().tcpNoDelay(true)); + out_points_.reset(new pcl::PointCloud); } void Convert::callback(rslidar_pointcloud::CloudNodeConfig& config, uint32_t level) @@ -52,23 +53,22 @@ void Convert::callback(rslidar_pointcloud::CloudNodeConfig& config, uint32_t lev /** @brief Callback for raw scan messages. */ void Convert::processScan(const rslidar_msgs::rslidarScan::ConstPtr& scanMsg) { - pcl::PointCloud::Ptr outPoints(new pcl::PointCloud); - outPoints->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; - outPoints->header.frame_id = scanMsg->header.frame_id; - outPoints->clear(); + out_points_->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; + out_points_->header.frame_id = scanMsg->header.frame_id; + out_points_->clear(); if (model == "RS16") { - outPoints->height = 16; - outPoints->width = 24 * (int)scanMsg->packets.size(); - outPoints->is_dense = false; - outPoints->resize(outPoints->height * outPoints->width); + out_points_->height = 16; + out_points_->width = 24 * (int)scanMsg->packets.size(); + out_points_->is_dense = false; + out_points_->resize(out_points_->height * out_points_->width); } else if (model == "RS32") { - outPoints->height = 32; - outPoints->width = 12 * (int)scanMsg->packets.size(); - outPoints->is_dense = false; - outPoints->resize(outPoints->height * outPoints->width); + out_points_->height = 32; + out_points_->width = 12 * (int)scanMsg->packets.size(); + out_points_->is_dense = false; + out_points_->resize(out_points_->height * out_points_->width); } // process each packet provided by the driver @@ -76,11 +76,11 @@ void Convert::processScan(const rslidar_msgs::rslidarScan::ConstPtr& scanMsg) data_->block_num = 0; for (size_t i = 0; i < scanMsg->packets.size(); ++i) { - data_->unpack(scanMsg->packets[i], outPoints); + data_->unpack(scanMsg->packets[i], out_points_); } - sensor_msgs::PointCloud2 outMsg; - pcl::toROSMsg(*outPoints, outMsg); - output_.publish(outMsg); + pcl::toROSMsg(*out_points_, out_msg_); + + output_.publish(out_msg_); } } // namespace rslidar_pointcloud diff --git a/rslidar_pointcloud/src/convert.h b/rslidar_pointcloud/src/convert.h index 93df091..c5681f6 100644 --- a/rslidar_pointcloud/src/convert.h +++ b/rslidar_pointcloud/src/convert.h @@ -44,6 +44,9 @@ class Convert boost::shared_ptr data_; ros::Subscriber rslidar_scan_; ros::Publisher output_; + + pcl::PointCloud::Ptr out_points_; + sensor_msgs::PointCloud2 out_msg_; }; } // namespace rslidar_pointcloud