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

Ajout d'options au launch file

Browse source 
Reste a regler le problème de queue ROS -> lent ...
This commit is contained in:
Unknown 2018-08-03 14:57:20 +02:00
parent 3a7050eb53
commit ced3ccd8cd
5 changed files with 23 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

4
asift_match/launch/asift_match.launch
View file

@ -1,5 +1,7 @@
<launch> <launch>
<param name="reference_txt_path" value="$(find asift_matching)/reference_data/references.txt"/> <param name="tracked_object" value="6DOF"/>
<param name="num_tilt" type="int" value="2"/>
<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>
<!--rosparam param="reference_data">[ <!--rosparam param="reference_data">[
"$(find asift_matching)/reference_data/train_image_000.png", "$(find asift_matching)/reference_data/train_image_000.png",
"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam--> "$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->

6
asift_match/launch/asift_match.launch~
View file

@ -1,8 +1,10 @@
<launch> <launch>
<param name="tracked_object" value="6DOF"/>
<param name="num_tilt" type="int" value="2"/>
<param name="reference_txt_path" value="$(find asift_matching)/reference_data/references.txt"/> <param name="reference_txt_path" value="$(find asift_matching)/reference_data/references.txt"/>
<rosparam param="reference_data">[ <!--rosparam param="reference_data">[
"$(find asift_matching)/reference_data/train_image_000.png", "$(find asift_matching)/reference_data/train_image_000.png",
"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam> "$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->
<param name="object_center_topic" value="/object_center"/> <param name="object_center_topic" value="/object_center"/>
<param name="image_topic" value="/camera/rgb/image_raw"/> <param name="image_topic" value="/camera/rgb/image_raw"/>

0
asift_match/reference_data/references.txt → asift_match/reference_data/book_references.txt
View file

19
asift_match/src/ROS_matcher.cpp
View file

@ -1,22 +1,25 @@
#include "ROS_matcher.hpp" #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::string center_topic, image_topic, pointcloud_topic, reference_txt_path;
std::vector<std::string> reference_data_paths; std::vector<std::string> reference_data_paths;
_nh.param<std::string>("tracked_object", tracked_object, "6DOF");
_nh.param<int>("num_tilt", _num_tilt, 8);
_nh.param<std::string>("object_center_topic", center_topic,"/ASIFT_matcher/object_center"); _nh.param<std::string>("object_center_topic", center_topic,"/ASIFT_matcher/object_center");
_nh.param<std::string>("image_topic", image_topic,"/camera/rgb/image_raw"); _nh.param<std::string>("image_topic", image_topic,"/camera/rgb/image_raw");
_nh.param<std::string>("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points"); _nh.param<std::string>("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points");
_center_pub = _nh.advertise<rviz_interface::NamedPoint>(center_topic, 1); _center_pub = _nh.advertise<rviz_interface::NamedPoint>(center_topic, 1);
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, image_topic, 1); image_sub = new message_filters::Subscriber<sensor_msgs::Image>(_nh, image_topic, 1);
pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, pointcloud_topic, 1); pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, pointcloud_topic, 1);
Timesync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),*info_sub, *image_sub, *pointcloud_sub); Timesync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(2), *image_sub, *pointcloud_sub);
Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2, _3)); Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2));
if(_nh.getParam("reference_txt_path", reference_txt_path)) 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; i<reference_data_paths.size(); i++) for(unsigned int i=0; i<reference_data_paths.size(); i++)
matcher.addReference(reference_data_paths[i].c_str(),_num_tilt); matcher.addReference(reference_data_paths[i].c_str(),_num_tilt);
if(matcher.getNbRef()>0) if(matcher.getNbRef()>0)
_status = MATCHER_STATUS_IDLE; _status = MATCHER_STATUS_IDLE;
} }
@ -42,13 +45,13 @@ ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_WAITING_INIT)
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, 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"); // 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)) 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.header.frame_id = image_msg->header.frame_id;
center_msg.point.x=center.x; center_msg.point.x=center.x;

8
asift_match/src/ROS_matcher.hpp
View file

@ -19,7 +19,7 @@
#include "ASIFT_matcher.hpp" #include "ASIFT_matcher.hpp"
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
enum MATCHER_STATUS{ enum MATCHER_STATUS{
MATCHER_STATUS_IDLE=0, MATCHER_STATUS_IDLE=0,
@ -36,7 +36,7 @@ protected:
ros::Publisher _center_pub; ros::Publisher _center_pub;
//Subscriber ROS //Subscriber ROS
message_filters::Subscriber<sensor_msgs::CameraInfo>* info_sub; // message_filters::Subscriber<sensor_msgs::CameraInfo>* info_sub;
message_filters::Subscriber<sensor_msgs::Image>* image_sub; message_filters::Subscriber<sensor_msgs::Image>* image_sub;
message_filters::Subscriber<sensor_msgs::PointCloud2>* pointcloud_sub; message_filters::Subscriber<sensor_msgs::PointCloud2>* pointcloud_sub;
@ -48,10 +48,12 @@ protected:
message_filters::Synchronizer<MySyncPolicy>* Timesync; message_filters::Synchronizer<MySyncPolicy>* Timesync;
std::string tracked_object;
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, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg); void cameraCallback(const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg);
}; };
#endif #endif