mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 17:27:46 +08:00
89 lines
2.7 KiB
C++
89 lines
2.7 KiB
C++
|
|
#include <ros/ros.h>
|
|
#include <pluginlib/class_list_macros.h>
|
|
#include <nodelet/nodelet.h>
|
|
|
|
#include <pcl/point_cloud.h>
|
|
#include <pcl/point_types.h>
|
|
#include <pcl_conversions/pcl_conversions.h>
|
|
|
|
#include <tf/transform_listener.h>
|
|
|
|
#include <sensor_msgs/PointCloud2.h>
|
|
|
|
#include <image_transport/image_transport.h>
|
|
#include <image_transport/subscriber_filter.h>
|
|
|
|
#include <message_filters/sync_policies/approximate_time.h>
|
|
#include <message_filters/subscriber.h>
|
|
#include <message_filters/sync_policies/approximate_time.h>
|
|
|
|
#include <rtabmap_ros/MsgConversion.h>
|
|
|
|
namespace rtabmap_ros
|
|
{
|
|
|
|
class PointCloudAggregator : public nodelet::Nodelet
|
|
{
|
|
public:
|
|
PointCloudAggregator() : sync(NULL)
|
|
{}
|
|
|
|
virtual ~PointCloudAggregator()
|
|
{
|
|
if (sync!=NULL) delete sync;
|
|
}
|
|
|
|
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()
|
|
{
|
|
ros::NodeHandle & nh = getNodeHandle();
|
|
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
|
|
|
int queueSize = 5;
|
|
pnh.param("queue_size", queueSize, queueSize);
|
|
|
|
cloudSub_1_.subscribe(nh, "cloud1", 1);
|
|
cloudSub_2_.subscribe(nh, "cloud2", 1);
|
|
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);
|
|
}
|
|
|
|
message_filters::Synchronizer<MySyncPolicy>* sync;
|
|
message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
|
|
message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
|
|
message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
|
|
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;
|
|
|
|
ros::Publisher cloudPub_;
|
|
};
|
|
|
|
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
|
|
}
|
|
|