AntoineH/BaxterInterface
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Synchro avec pointCloud

Browse source
This commit is contained in:
Unknown 2018-08-02 11:31:09 +02:00
parent ffe3eee32b
commit 36abb2ffd7
2 changed files with 22 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

22
asift_match/src/ROS_matcher.cpp
View file

@ -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); 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); 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); 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 = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),*info_sub, *image_sub, *pointcloud_sub);
Timesync->registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2)); Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2, _3));
// unsigned int nb_ref =2; // unsigned int nb_ref =2;
// std::string refData[] = { // std::string refData[] = {
@ -24,13 +24,13 @@ ROS_matcher::ROS_matcher(): _num_tilt(1), _status(MATCHER_STATUS_IDLE)
ROS_matcher::~ROS_matcher() ROS_matcher::~ROS_matcher()
{ {
// delete info_sub; delete info_sub;
// delete image_sub; delete image_sub;
// delete pointcloud_sub; delete pointcloud_sub;
// delete Timesync; // 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"); ROS_INFO("Callback");
@ -72,14 +72,20 @@ void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_m
//Recherche du point 3D //Recherche du point 3D
if(nb_match>0) if(nb_match>0)
{ {
//Publish 3D point //Publish 3D point
int cx, cy; int cx, cy;
geometry_msgs::PointStamped center_msg; geometry_msgs::PointStamped center_msg;
//Provisoire //Provisoire
if(matcher.computeCenter(cx, cy)) 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.header.frame_id = image_msg->header.frame_id;
center_msg.point.x=cx; center_msg.point.x=cx;
center_msg.point.y=cy; center_msg.point.y=cy;

11
asift_match/src/ROS_matcher.hpp
View file

@ -5,15 +5,20 @@
#include <tf/tf.h> #include <tf/tf.h>
#include <message_filters/subscriber.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/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/PointStamped.h>
#include <pcl_conversions/pcl_conversions.h>
#include "ASIFT_matcher.hpp" #include "ASIFT_matcher.hpp"
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
enum MATCHER_STATUS{ enum MATCHER_STATUS{
MATCHER_STATUS_IDLE=0, MATCHER_STATUS_IDLE=0,
MATCHER_STATUS_PROCESSING, MATCHER_STATUS_PROCESSING,
@ -39,12 +44,12 @@ protected:
MATCHER_STATUS _status; MATCHER_STATUS _status;
message_filters::TimeSynchronizer<sensor_msgs::CameraInfo, sensor_msgs::Image>* Timesync; message_filters::Synchronizer<MySyncPolicy>* Timesync;
public: public:
ROS_matcher(); ROS_matcher();
~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 #endif