diff --git a/mergeScans.cpp b/mergeScans.cpp new file mode 100644 index 0000000..9a45bad --- /dev/null +++ b/mergeScans.cpp @@ -0,0 +1,58 @@ +#include "mergeScans.h" + +mergeScans::mergeScans(ros::NodeHandle *nh) : sync_(mergeScans::mergeScansSyncPolicy_(2), this->zed2Cloud_, this->zedmCloud_), fusedCloud_((new pcl::PointCloud)) +{ + this->zed2Cloud_.subscribe(*nh, "/zed2/zed2_node/point_cloud/cloud_registered", 2); + this->zedmCloud_.subscribe(*nh, "/zedm/zedm_node/point_cloud/cloud_registered", 2); + this->mergedCloud_ = nh->advertise("/zed2/zed2_node/point_cloud/cloud_merged", 1); + this->sync_.registerCallback(std::bind(&mergeScans::callBack_, this, std::placeholders::_1, std::placeholders::_2)); +} + +mergeScans::~mergeScans() +{ +} + +void mergeScans::callBack_(const sensor_msgs::PointCloud2::ConstPtr& zed2CloudMsg, const sensor_msgs::PointCloud2::ConstPtr& zedmCloudMsg) +{ + pcl::PointCloud::Ptr zed2Cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr zedmCloud(new pcl::PointCloud); + pcl::fromROSMsg(*zed2CloudMsg, *zed2Cloud); + pcl::fromROSMsg(*zedmCloudMsg, *zedmCloud); + + *(this->fusedCloud_) = *zed2Cloud + *(this->transformCloud_(zedmCloud)); + + //ROS messages + sensor_msgs::PointCloud2 finalCloud; + pcl::toROSMsg(*(this->fusedCloud_), finalCloud); + finalCloud.header = zed2CloudMsg->header; + this->mergedCloud_.publish(finalCloud); + +} + +pcl::PointCloud::Ptr mergeScans::transformCloud_(const pcl::PointCloud::Ptr& localCloud) +{ + pcl::PointCloud::Ptr output(new pcl::PointCloud); + output->resize(localCloud->size()); + Eigen::Matrix4f transformation; + transformation << -1.0 , 0.0 , 0.0 , -0.602, + 0.0 , -1.0 , 0.0 , 0.0, + 0.0 , 0.0 , 1.0 , 0.0, + 0 , 0 , 0 , 1 ; + + pcl::transformPointCloud(*localCloud, *output, transformation); + + return output; +} + + +//main +int main(int argc, char** argv) +{ + ros::init(argc, argv, "merge_scans"); + ros::NodeHandle nh; + mergeScans* mergeScans_ = new mergeScans(&nh); + ros::MultiThreadedSpinner spinner(12); + spinner.spin(); + + return 0; +} \ No newline at end of file diff --git a/mergeScans.h b/mergeScans.h new file mode 100644 index 0000000..2f53804 --- /dev/null +++ b/mergeScans.h @@ -0,0 +1,44 @@ +#ifndef MERGESCANS_H_ +#define MERGESCANS_H_ + +//ROS includes +#include +#include + +//all sensor includes +#include + +//message filter includes +#include +#include +#include +#include + +//pcl includes +#include +#include +#include +#include +#include +#include +#include +#include + +class mergeScans +{ +private: + typedef message_filters::sync_policies::ApproximateTime mergeScansSyncPolicy_; + message_filters::Subscriber zed2Cloud_; + message_filters::Subscriber zedmCloud_; + message_filters::Synchronizer sync_; + ros::Publisher mergedCloud_; + void callBack_(const sensor_msgs::PointCloud2::ConstPtr& zed2CloudMsg, const sensor_msgs::PointCloud2::ConstPtr& zedmCloudMsg); + pcl::PointCloud::Ptr transformCloud_(const pcl::PointCloud::Ptr& localCloud); + pcl::PointCloud::Ptr fusedCloud_; +public: + mergeScans(ros::NodeHandle *nh); + ~mergeScans(); +}; + + +#endif \ No newline at end of file