From ced3ccd8cd9c894f3b1bfe28b8d99169e3b1f537 Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 3 Aug 2018 14:57:20 +0200 Subject: [PATCH] Ajout d'options au launch file MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Reste a regler le problème de queue ROS -> lent ... --- asift_match/launch/asift_match.launch | 4 +++- asift_match/launch/asift_match.launch~ | 6 ++++-- .../{references.txt => book_references.txt} | 0 asift_match/src/ROS_matcher.cpp | 19 +++++++++++-------- asift_match/src/ROS_matcher.hpp | 8 +++++--- 5 files changed, 23 insertions(+), 14 deletions(-) rename asift_match/reference_data/{references.txt => book_references.txt} (100%) diff --git a/asift_match/launch/asift_match.launch b/asift_match/launch/asift_match.launch index 110b174..1655b27 100644 --- a/asift_match/launch/asift_match.launch +++ b/asift_match/launch/asift_match.launch @@ -1,5 +1,7 @@ - + + + diff --git a/asift_match/launch/asift_match.launch~ b/asift_match/launch/asift_match.launch~ index 870dba3..882922c 100644 --- a/asift_match/launch/asift_match.launch~ +++ b/asift_match/launch/asift_match.launch~ @@ -1,8 +1,10 @@ + + - [ + diff --git a/asift_match/reference_data/references.txt b/asift_match/reference_data/book_references.txt similarity index 100% rename from asift_match/reference_data/references.txt rename to asift_match/reference_data/book_references.txt diff --git a/asift_match/src/ROS_matcher.cpp b/asift_match/src/ROS_matcher.cpp index ffda8fe..ae8ebc4 100644 --- a/asift_match/src/ROS_matcher.cpp +++ b/asift_match/src/ROS_matcher.cpp @@ -1,22 +1,25 @@ #include "ROS_matcher.hpp" -ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_WAITING_INIT) +ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT) { std::string center_topic, image_topic, pointcloud_topic, reference_txt_path; std::vector reference_data_paths; + _nh.param("tracked_object", tracked_object, "6DOF"); + _nh.param("num_tilt", _num_tilt, 8); + _nh.param("object_center_topic", center_topic,"/ASIFT_matcher/object_center"); _nh.param("image_topic", image_topic,"/camera/rgb/image_raw"); _nh.param("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points"); _center_pub = _nh.advertise(center_topic, 1); - info_sub = new message_filters::Subscriber(_nh, "camera/rgb/camera_info", 1); + // info_sub = new message_filters::Subscriber(_nh, "camera/rgb/camera_info", 1); image_sub = new message_filters::Subscriber(_nh, image_topic, 1); pointcloud_sub= new message_filters::Subscriber(_nh, pointcloud_topic, 1); - Timesync = new message_filters::Synchronizer(MySyncPolicy(10),*info_sub, *image_sub, *pointcloud_sub); - Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2, _3)); + Timesync = new message_filters::Synchronizer(MySyncPolicy(2), *image_sub, *pointcloud_sub); + Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2)); if(_nh.getParam("reference_txt_path", reference_txt_path)) { @@ -27,7 +30,7 @@ ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_WAITING_INIT) { for(unsigned int i=0; i0) _status = MATCHER_STATUS_IDLE; } @@ -42,13 +45,13 @@ ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_WAITING_INIT) ROS_matcher::~ROS_matcher() { - delete info_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, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg) +void ROS_matcher::cameraCallback(const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg) { // ROS_INFO("Callback"); @@ -116,7 +119,7 @@ void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_m if(!isnan(center.x) && !isnan(center.y) && !isnan(center.z)) { - center_msg.name = "6DOF"; + center_msg.name = tracked_object; center_msg.header.frame_id = image_msg->header.frame_id; center_msg.point.x=center.x; diff --git a/asift_match/src/ROS_matcher.hpp b/asift_match/src/ROS_matcher.hpp index e64c242..6c5c690 100644 --- a/asift_match/src/ROS_matcher.hpp +++ b/asift_match/src/ROS_matcher.hpp @@ -19,7 +19,7 @@ #include "ASIFT_matcher.hpp" -typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; +typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; enum MATCHER_STATUS{ MATCHER_STATUS_IDLE=0, @@ -36,7 +36,7 @@ protected: ros::Publisher _center_pub; //Subscriber ROS - message_filters::Subscriber* info_sub; + // message_filters::Subscriber* info_sub; message_filters::Subscriber* image_sub; message_filters::Subscriber* pointcloud_sub; @@ -48,10 +48,12 @@ protected: message_filters::Synchronizer* Timesync; + std::string tracked_object; + public: ROS_matcher(); ~ROS_matcher(); - void cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_msg, const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg); + void cameraCallback(const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg); }; #endif \ No newline at end of file