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

Updated point_cloud_aggregator to transform clouds in same frame

Browse Source
This commit is contained in:
matlabbe
2017-03-17 11:09:10 -04:00
parent 22c63924ca
commit ad32b6bee8
1 changed files with 31 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_aggregator.cpp
+31 -7
View File
@@ -7,6 +7,8 @@
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
@@ -26,12 +28,12 @@ namespace rtabmap_ros
class PointCloudAggregator : public nodelet::Nodelet class PointCloudAggregator : public nodelet::Nodelet
{ {
public: public:
PointCloudAggregator() : sync(NULL) PointCloudAggregator() : sync_(NULL)
{} {}
virtual ~PointCloudAggregator() virtual ~PointCloudAggregator()
{ {
if (sync!=NULL) delete sync; if (sync_!=NULL) delete sync_;
} }
private: private:
@@ -41,16 +43,35 @@ private:
{ {
if(cloudPub_.getNumSubscribers()) if(cloudPub_.getNumSubscribers())
{ {
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;
pcl::fromROSMsg(*cloudMsg_1, cloud1); pcl::fromROSMsg(*cloudMsg_1, cloud1);
std::string frameId = frameId_;
if(!frameId.empty() && frameId.compare(cloudMsg_1->header.frame_id) != 0)
{
pcl_ros::transformPointCloud(frameId, cloud1, cloud1, tfListener_);
}
else
{
frameId = cloudMsg_1->header.frame_id;
}
pcl::fromROSMsg(*cloudMsg_2, cloud2); pcl::fromROSMsg(*cloudMsg_2, cloud2);
if(frameId.compare(cloudMsg_2->header.frame_id) != 0)
{
pcl_ros::transformPointCloud(frameId, cloud2, cloud2, tfListener_);
}
pcl::fromROSMsg(*cloudMsg_3, cloud3); pcl::fromROSMsg(*cloudMsg_3, cloud3);
if(frameId.compare(cloudMsg_3->header.frame_id) != 0)
{
pcl_ros::transformPointCloud(frameId, cloud3, cloud3, tfListener_);
}
pcl::PointCloud<pcl::PointXYZ> totalCloud; pcl::PointCloud<pcl::PointXYZ> totalCloud;
totalCloud = cloud1 + cloud2; totalCloud = cloud1 + cloud2;
totalCloud += cloud3; totalCloud += cloud3;
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(totalCloud, rosCloud); pcl::toROSMsg(totalCloud, rosCloud);
rosCloud.header.stamp = cloudMsg_1->header.stamp; rosCloud.header.stamp = cloudMsg_1->header.stamp;
rosCloud.header.frame_id = cloudMsg_1->header.frame_id; rosCloud.header.frame_id = frameId;
cloudPub_.publish(rosCloud); cloudPub_.publish(rosCloud);
} }
} }
@@ -63,24 +84,27 @@ private:
int queueSize = 5; int queueSize = 5;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_);
cloudSub_1_.subscribe(nh, "cloud1", 1); cloudSub_1_.subscribe(nh, "cloud1", 1);
cloudSub_2_.subscribe(nh, "cloud2", 1); cloudSub_2_.subscribe(nh, "cloud2", 1);
cloudSub_3_.subscribe(nh, "cloud3", 1); cloudSub_3_.subscribe(nh, "cloud3", 1);
sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3)); sync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3));
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1); cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
} }
message_filters::Synchronizer<MySyncPolicy>* sync; message_filters::Synchronizer<MySyncPolicy>* sync_;
message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_; message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_; message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_; message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;
ros::Publisher cloudPub_; ros::Publisher cloudPub_;
std::string frameId_;
tf::TransformListener tfListener_;
}; };
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);