From 2c37545509a86dd11a1f8a29e7570b56e0fc3d97 Mon Sep 17 00:00:00 2001 From: ruffsl Date: Fri, 19 Feb 2016 02:28:37 -0500 Subject: [PATCH 1/3] Fixing Topic issue Just having the list used to simply subscribe, no fancy filtering topics, trusting the user to correctly list the topics. Otherwise we'd need to recall the topic parsing, and make sure our topic is available before the recall. This originally didn't play nice with a slow gazebo startup. --- src/laserscan_multi_merger.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/laserscan_multi_merger.cpp b/src/laserscan_multi_merger.cpp index ad739e5..dbfb46b 100644 --- a/src/laserscan_multi_merger.cpp +++ b/src/laserscan_multi_merger.cpp @@ -71,9 +71,6 @@ void LaserscanMerger::reconfigureCallback(laserscan_multi_mergerConfig &config, void LaserscanMerger::laserscan_topic_parser() { // LaserScan topics to subscribe - ros::master::V_TopicInfo topics; - ros::master::getTopics(topics); - istringstream iss(laserscan_topics); vector tokens; copy(istream_iterator(iss), istream_iterator(), back_inserter >(tokens)); @@ -82,13 +79,7 @@ void LaserscanMerger::laserscan_topic_parser() for(int i=0;i Date: Fri, 19 Feb 2016 02:30:30 -0500 Subject: [PATCH 2/3] Switching the using latest scan's time stamp so sensor data timing is better preserved --- src/laserscan_multi_merger.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/laserscan_multi_merger.cpp b/src/laserscan_multi_merger.cpp index dbfb46b..313634b 100644 --- a/src/laserscan_multi_merger.cpp +++ b/src/laserscan_multi_merger.cpp @@ -25,7 +25,7 @@ class LaserscanMerger public: LaserscanMerger(); void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic); - void pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud); + void pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud, const sensor_msgs::LaserScan::ConstPtr& scan); void reconfigureCallback(laserscan_multi_mergerConfig &config, uint32_t level); private: @@ -178,16 +178,16 @@ void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, Eigen::MatrixXf points; getPointCloudAsEigen(merged_cloud,points); - pointcloud_to_laserscan(points, &merged_cloud); + pointcloud_to_laserscan(points, &merged_cloud, scan); } } -void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud) +void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud, const sensor_msgs::LaserScan::ConstPtr& scan) { sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan()); output->header = pcl_conversions::fromPCL(merged_cloud->header); output->header.frame_id = destination_frame.c_str(); - output->header.stamp = ros::Time::now(); //fixes #265 + output->header.stamp = scan->header.stamp; output->angle_min = this->angle_min; output->angle_max = this->angle_max; output->angle_increment = this->angle_increment; From 271f8cd451b0d1730019d2063338868fb85ed283 Mon Sep 17 00:00:00 2001 From: ruffsl Date: Fri, 19 Feb 2016 02:37:55 -0500 Subject: [PATCH 3/3] Fixing issue with pointcloud field mismatch When combining a synthetic scan from a 2.5D asus camera and 2D hokuyo, one might get this error message: `[pcl::concatenatePointCloud] Number of fields in cloud1 (4) != Number of fields in cloud2 (5)` By making it explicit to just combine distance, we can avoide this issue. --- src/laserscan_multi_merger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/laserscan_multi_merger.cpp b/src/laserscan_multi_merger.cpp index 313634b..4944e1c 100644 --- a/src/laserscan_multi_merger.cpp +++ b/src/laserscan_multi_merger.cpp @@ -139,7 +139,7 @@ void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, // Verify that TF knows how to transform from the received scan to the destination scan frame tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1)); - projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_); + projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_, laser_geometry::channel_option::Distance); try { tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);