Ajout d'options au launch file (Interface Rviz)

asift_match/src/ROS_matcher.cpp

@@ -5,6 +5,7 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT)
 	std::string center_topic, image_topic, pointcloud_topic, reference_txt_path;
 	std::vector<std::string> reference_data_paths;
 
+	//Load Param
 	_nh.param<std::string>("tracked_object", tracked_object, "6DOF");
 	_nh.param<int>("num_tilt", _num_tilt, 8);
 
@@ -12,8 +13,10 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT)
 	_nh.param<std::string>("image_topic", image_topic,"/camera/rgb/image_raw");
 	_nh.param<std::string>("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points");
 	
+	//Publisher
 	_center_pub = _nh.advertise<rviz_interface::NamedPoint>(center_topic, 1);
 
+	//Subscriber
 	// 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);
 	pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, pointcloud_topic, 1);
@@ -21,6 +24,7 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT)
 	Timesync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(2), *image_sub, *pointcloud_sub);
 	Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2));
 
+	//Load References
 	if(_nh.getParam("reference_txt_path", reference_txt_path))
 	{
 		if(matcher.loadReferences(reference_txt_path.c_str()))

rviz_interface/rviz_interface/CMakeLists.txt

@@ -9,6 +9,11 @@ project(rviz_interface)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf   rospy std_msgs message_generation genmsg)
 
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################

rviz_interface/rviz_interface/launch/interface_matching.launch

@@ -1,5 +1,24 @@
 <launch>
 	<node name="rviz" pkg="rviz" type="rviz"/>
+
+	<!--Interface/-->
+	<param name="objective_topic" value="/RvizInterface/state_objective"/>
+	<param name="vizualization_topic" value="/RvizInterface/visual_marker"/>
+	<param name="config_topic" value="/RvizInterface/interface_config"/>
+
 	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
-	<!--node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher"/-->
+
+	<!--Matcher/-->
+	<param name="tracked_object" value="Foo"/>
+	<param name="num_tilt" type="int" value="8"/>
+	<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>
+	<!--rosparam param="reference_data">[
+	"$(find asift_matching)/reference_data/train_image_000.png", 
+	"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->
+
+	<param name="object_center_topic" value="/object_center"/>
+	<param name="image_topic" value="/camera/rgb/image_raw"/>
+	<param name="pointcloud_topic" value="/camera/depth_registered/points"/>
+
+	<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher" output="screen"/>
 </launch>

rviz_interface/rviz_interface/launch/interface_matching.launch~

@@ -1,5 +1,24 @@
 <launch>
 	<node name="rviz" pkg="rviz" type="rviz"/>
-	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
+
-	<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher"/>
+	<!--Interface/-->
+	<param name="objective_topic" value="/RvizInterface/state_objective"/>
+	<param name="vizualization_topic" value="/RvizInterface/visual_marker"/>
+	<param name="config_topic" value="/RvizInterface/interface_config"/>
+
+	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface" output="screen"/>
+
+	<!--Matcher/-->
+	<param name="tracked_object" value="Foo"/>
+	<param name="num_tilt" type="int" value="8"/>
+	<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>
+	<!--rosparam param="reference_data">[
+	"$(find asift_matching)/reference_data/train_image_000.png", 
+	"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->
+
+	<param name="object_center_topic" value="/object_center"/>
+	<param name="image_topic" value="/camera/rgb/image_raw"/>
+	<param name="pointcloud_topic" value="/camera/depth_registered/points"/>
+
+	<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher" output="screen"/>
 </launch>

rviz_interface/rviz_interface/setup.py

@@ -0,0 +1,17 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+	packages=['asift_matching'],
+	package_dir={'': 'src'},
+	)
+
+setup(**setup_args)
+
+

rviz_interface/rviz_interface/src/RvizInterface.cpp

@@ -3,20 +3,30 @@
 //Constructeur
 RvizInterface::RvizInterface(): _server("RvizInterface")
 {
+	std::string objective_topic, vizualization_topic, config_topic, position_topic;
+	//Need to load multiple object
+	std::string object_name;
+
+	//Load Param
+	_n.param<std::string>("objective_topic", objective_topic,"/RvizInterface/state_objective");
+	_n.param<std::string>("vizualization_topic", vizualization_topic,"/RvizInterface/visual_marker");
+	_n.param<std::string>("config_topic", config_topic,"/RvizInterface/interface_config");
+	_n.param<std::string>("object_center_topic", position_topic,"/object_center");
+	_n.param<std::string>("tracked_object", object_name,"Object");
+
 	//Topic you want to publish
-	_objective_pub = _n.advertise<rviz_interface::StateSpace>("/RvizInterface/state_objective", 10);
+	_objective_pub = _n.advertise<rviz_interface::StateSpace>(objective_topic, 10);
-	_visualization_pub = _n.advertise<visualization_msgs::Marker>("/RvizInterface/visual_marker", 10);
+	_visualization_pub = _n.advertise<visualization_msgs::Marker>(vizualization_topic, 10);
 
 	//Topic you want to subscribe
-	_config_sub = _n.subscribe("/RvizInterface/interface_config", 1, &RvizInterface::configCallback, this);
+	_config_sub = _n.subscribe(config_topic, 1, &RvizInterface::configCallback, this);
-	_position_sub = _n.subscribe("/object_center", 1, &RvizInterface::positionCallback, this);
+	_position_sub = _n.subscribe(position_topic, 1, &RvizInterface::positionCallback, this);
 
-	_objects.push_back(new InteractiveObject(&_server, "6DOF", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
+	_objects.push_back(new InteractiveObject(&_server, object_name, (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 	_objects[0]->add6DOFcontrol();
 
-	_objects.push_back(new InteractiveObject(&_server, "3DOF", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,1,0)));
+	// _objects.push_back(new InteractiveObject(&_server, "3DOF", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,1,0)));
-	_objects[1]->add3DOFcontrol();
+	// _objects[1]->add3DOFcontrol();
-
 
 	// _objects.push_back(new InteractiveObject(&_server, "3DOF 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(-1,1,0)));
 	// _objects[2]->add3DOFcontrol(tf::Vector3(0.5,0.5,0));