i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

icp_odometry: publishing odom_filtered_input_scan debugging topic. Added test_velodyne.launch

Browse Source
This commit is contained in:
matlabbe
2019-02-21 19:38:34 -05:00
parent 1f597a58d0
commit 839d7d2835
2 changed files with 164 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+42
View File
@@ -96,6 +96,8 @@ private:
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -229,10 +231,26 @@ private:
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScanNormal, msg);
msg.header = scanMsg->header;
filtered_scan_pub_.publish(msg);
}
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = scanMsg->header;
filtered_scan_pub_.publish(msg);
}
}
}
@@ -285,6 +303,13 @@ private:
maxLaserScans /= scanDownsamplingStep_;
}
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg->header;
filtered_scan_pub_.publish(msg);
}
}
else
{
@@ -316,10 +341,26 @@ private:
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScanNormal, msg);
msg.header = cloudMsg->header;
filtered_scan_pub_.publish(msg);
}
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg->header;
filtered_scan_pub_.publish(msg);
}
}
}
}
@@ -344,6 +385,7 @@ protected:
private:
ros::Subscriber scan_sub_;
ros::Subscriber cloud_sub_;
ros::Publisher filtered_scan_pub_;
int scanCloudMaxPoints_;
int scanDownsamplingStep_;
double scanVoxelSize_;