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

sync

Browse Source
This commit is contained in:
bibarz
2015-06-15 14:29:19 -07:00
parent c39f407b91
commit c5be6bd934
1 changed files with 37 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_aggregator.cpp
+37 -49
View File
@@ -16,6 +16,7 @@
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h> #include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <rtabmap_ros/MsgConversion.h> #include <rtabmap_ros/MsgConversion.h>
@@ -25,71 +26,58 @@ namespace rtabmap_ros
class PointCloudAggregator : public nodelet::Nodelet class PointCloudAggregator : public nodelet::Nodelet
{ {
public: public:
PointCloudAggregator() PointCloudAggregator() : sync(NULL)
{} {}
virtual ~PointCloudAggregator() virtual ~PointCloudAggregator()
{} {
if (sync!=NULL) delete sync;
}
private: private:
void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
{
if(cloudPub_.getNumSubscribers())
{
pcl::fromROSMsg(*cloudMsg_1, cloud1);
pcl::fromROSMsg(*cloudMsg_2, cloud2);
pcl::fromROSMsg(*cloudMsg_3, cloud3);
pcl::PointCloud<pcl::PointXYZ> totalCloud;
totalCloud = cloud1 + cloud2;
totalCloud += cloud3;
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(totalCloud, rosCloud);
rosCloud.header.stamp = cloudMsg_1->header.stamp;
rosCloud.header.frame_id = cloudMsg_1->header.frame_id;
cloudPub_.publish(rosCloud);
}
}
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> MySyncPolicy;
virtual void onInit() virtual void onInit()
{ {
ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 5;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
cloudSub_1_ = nh.subscribe("cloud1", 1, &PointCloudAggregator::callback_1, this); cloudSub_1_.subscribe(nh, "cloud1", 1);
cloudSub_2_ = nh.subscribe("cloud2", 1, &PointCloudAggregator::callback_2, this); cloudSub_2_.subscribe(nh, "cloud2", 1);
cloudSub_3_ = nh.subscribe("cloud3", 1, &PointCloudAggregator::callback_3, this); cloudSub_3_.subscribe(nh, "cloud3", 1);
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));
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1); cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
} }
void callback_1(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) message_filters::Synchronizer<MySyncPolicy>* sync;
{ message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
if(cloudPub_.getNumSubscribers()) message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
{ message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
pcl::fromROSMsg(*cloudMsg, cloud1);
gather_and_publish(cloudMsg);
}
}
void callback_2(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
if(cloudPub_.getNumSubscribers())
{
pcl::fromROSMsg(*cloudMsg, cloud2);
gather_and_publish(cloudMsg);
}
}
void callback_3(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
if(cloudPub_.getNumSubscribers())
{
pcl::fromROSMsg(*cloudMsg, cloud3);
gather_and_publish(cloudMsg);
}
}
void gather_and_publish(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
pcl::PointCloud<pcl::PointXYZ> totalCloud;
totalCloud = cloud1 + cloud2;
totalCloud += cloud3;
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(totalCloud, rosCloud);
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = cloudMsg->header.frame_id;
cloudPub_.publish(rosCloud);
}
private:
ros::Subscriber cloudSub_1_;
ros::Subscriber cloudSub_2_;
ros::Subscriber cloudSub_3_;
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3; pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;
ros::Publisher cloudPub_; ros::Publisher cloudPub_;