From 36abb2ffd743824b7851502e7a8170fe8a88923f Mon Sep 17 00:00:00 2001
From: Unknown <antoine.harle@etu.upmc.fr>
Date: Thu, 2 Aug 2018 11:31:09 +0200
Subject: [PATCH] Synchro avec pointCloud

---
 asift_match/src/ROS_matcher.cpp | 22 ++++++++++++++--------
 asift_match/src/ROS_matcher.hpp | 11 ++++++++---
 2 files changed, 22 insertions(+), 11 deletions(-)

diff --git a/asift_match/src/ROS_matcher.cpp b/asift_match/src/ROS_matcher.cpp
index 3e586a2..35edd11 100644
--- a/asift_match/src/ROS_matcher.cpp
+++ b/asift_match/src/ROS_matcher.cpp
@@ -7,8 +7,8 @@ ROS_matcher::ROS_matcher(): _num_tilt(1), _status(MATCHER_STATUS_IDLE)
 	info_sub = new message_filters::Subscriber<sensor_msgs::CameraInfo>(_nh, "/camera/rgb/camera_info", 1);
 	image_sub = new message_filters::Subscriber<sensor_msgs::Image>(_nh, "/camera/rgb/image_raw", 1);
 	pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, "/camera/depth_registered/points", 1);
-	Timesync = new message_filters::TimeSynchronizer<sensor_msgs::CameraInfo, sensor_msgs::Image>(*info_sub, *image_sub, 10);
-	Timesync->registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2));
+	Timesync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),*info_sub, *image_sub, *pointcloud_sub);
+	Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2, _3));
 
 	// unsigned int nb_ref =2;
 	// std::string refData[] = {
@@ -24,13 +24,13 @@ ROS_matcher::ROS_matcher(): _num_tilt(1), _status(MATCHER_STATUS_IDLE)
 
 ROS_matcher::~ROS_matcher()
 {
-	// delete info_sub;
-	// delete image_sub;
-	// delete pointcloud_sub;
+	delete info_sub;
+	delete image_sub;
+	delete pointcloud_sub;
 	// delete Timesync;
 }
 
-void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_msg, const sensor_msgs::Image::ConstPtr& image_msg)
+void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_msg, const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg)
 {
 	ROS_INFO("Callback");
 
@@ -72,14 +72,20 @@ void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_m
 		//Recherche du point 3D
 		if(nb_match>0)
 		{
-
-
 			//Publish 3D point
 			int cx, cy;
 			geometry_msgs::PointStamped center_msg;
 			//Provisoire
 			if(matcher.computeCenter(cx, cy))
 			{
+				//Conversions des donnée d'entrée en PCL
+				pcl::PCLPointCloud2 pcl_pc2;
+				pcl_conversions::toPCL(*pointcloud_msg,pcl_pc2);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::fromPCLPointCloud2(pcl_pc2,*cloud);
+
+				
+				
 				center_msg.header.frame_id = image_msg->header.frame_id;
 				center_msg.point.x=cx;
 				center_msg.point.y=cy;
diff --git a/asift_match/src/ROS_matcher.hpp b/asift_match/src/ROS_matcher.hpp
index 6ee921e..c7d6226 100644
--- a/asift_match/src/ROS_matcher.hpp
+++ b/asift_match/src/ROS_matcher.hpp
@@ -5,15 +5,20 @@
 #include <tf/tf.h>
 
 #include <message_filters/subscriber.h>
-#include <message_filters/time_synchronizer.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
 
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/Image.h>
 #include <geometry_msgs/PointStamped.h>
 
+#include <pcl_conversions/pcl_conversions.h>
+
 #include "ASIFT_matcher.hpp"
 
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
+
 enum MATCHER_STATUS{
 	MATCHER_STATUS_IDLE=0,
 	MATCHER_STATUS_PROCESSING,
@@ -39,12 +44,12 @@ protected:
 
 	MATCHER_STATUS _status;
 
-	message_filters::TimeSynchronizer<sensor_msgs::CameraInfo, sensor_msgs::Image>* Timesync;
+	message_filters::Synchronizer<MySyncPolicy>* Timesync;
 
 public:
 	ROS_matcher();
 	~ROS_matcher();
-	void cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_msg, const sensor_msgs::Image::ConstPtr& image_msg);
+	void cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_msg, const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg);
 	
 };
 #endif
\ No newline at end of file