Résolution du problème de frame_id des header (RvizInterface)

README.md

@@ -30,7 +30,6 @@ Thus refrences are required for the module to work properly. It can be loaded th
 ### Done
 
 ### Todo
-rviz_interface - InteractiveObject : Solve the issue with the frame_id, so it could be chosen in the launch file.
 asift_matching - ASIFT_matcher : Issue on total number of match (after filtering ?).
 asift_matching - ROS_matcher : Working loader of images for reference.
 asift_matching - ROS_matcher : Improve reactivity. Slow process due to the message queue length ?

README.md~

@@ -30,10 +30,11 @@ Thus refrences are required for the module to work properly. It can be loaded th
 ### Done
 
 ### Todo
-ASIFT_matcher : Issue on total number of match (after filtering ?).
+rviz_interface - InteractiveObject : Solve the issue with the frame_id, so it could be chosen in the launch file.
-ROS_matcher : Working loader of images for reference.
+asift_matching - ASIFT_matcher : Issue on total number of match (after filtering ?).
-ROS_matcher : Improve reactivity. Slow process due to the message queue length ?
+asift_matching - ROS_matcher : Working loader of images for reference.
-ROS_matcher : Swap the callback function to a service called for matching.
+asift_matching - ROS_matcher : Improve reactivity. Slow process due to the message queue length ?
+asift_matching - ROS_matcher : Swap the callback function to a service called for matching.
 
 ## How to use it ?
 

ongoing/pcl_tests/src/plannar_segmentation.cpp

@@ -92,7 +92,7 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
 
   // geometry_msgs::PointStamped center_msg;
   rviz_interface::NamedPoint center_msg;
-  center_msg.name = "6DOF";
+  center_msg.name = "foo";
   // for (size_t i = 0; i < pcl_output->size (); ++i)
   // {
   //   if( pcl_output->points[i].x)
@@ -134,7 +134,7 @@ int main (int argc, char** argv)
   // Create a ROS publisher for the output point cloud
   pub = nh.advertise<sensor_msgs::PointCloud2> ("/plane_output", 1);
   // pubc = nh.advertise<geometry_msgs::PointStamped> ("/object_center", 1);
-  pubc = nh.advertise<rviz_interface::NamedPoint> ("/RvizInterface/object_center", 1);
+  pubc = nh.advertise<rviz_interface::NamedPoint> ("/object_center", 1);
 
   // printinfo();
   // Spin

rviz_interface/launch/interface.launch

@@ -4,6 +4,7 @@
 	<!--Interface/-->
 		<!--Objects/-->
 	<param name="frame_id" value="/map"/>
+	<!--param name="frame_id" value="/camera_rgb_optical_frame"/-->
 	<rosparam param="object_names"> ["Foo", "KungFoo", "Panda", "Boo"]</rosparam>
 	<rosparam param="object_types"> [3, 2, 1, 0]</rosparam>
 

rviz_interface/launch/interface.launch~

@@ -3,6 +3,7 @@
 	
 	<!--Interface/-->
 		<!--Objects/-->
+	<param name="frame_id" value="/camera_rgb_optical_frame"/>
 	<rosparam param="object_names"> ["Foo", "KungFoo", "Panda", "Boo"]</rosparam>
 	<rosparam param="object_types"> [3, 2, 1, 0]</rosparam>
 

rviz_interface/launch/interface_matching.launch

@@ -3,6 +3,7 @@
 
 	<!--Interface/-->
 		<!--Objects/-->
+	<param name="frame_id" value="/camera_rgb_optical_frame"/>
 	<rosparam param="object_names"> ["Foo", "KungFoo", "Panda", "Boo"]</rosparam>
 	<rosparam param="object_types"> [3, 2, 1, 0]</rosparam>
 

rviz_interface/launch/interface_matching.launch~

@@ -3,6 +3,8 @@
 
 	<!--Interface/-->
 		<!--Objects/-->
+	<!--param name="frame_id" value="/map"/-->
+	<param name="frame_id" value="/camera_rgb_optical_frame"/>
 	<rosparam param="object_names"> ["Foo", "KungFoo", "Panda", "Boo"]</rosparam>
 	<rosparam param="object_types"> [3, 2, 1, 0]</rosparam>
 
@@ -17,7 +19,7 @@
 		<!--Matcher Parameters/-->
 	<param name="tracked_object" value="Foo"/>
 	<param name="num_tilt" type="int" value="8"/>
-	<param name="std_dev_filter_coeff" type="double" value="0.5"/>
+	<param name="std_dev_filter_coeff" type="double" value="2"/>
 		<!--Matcher References/-->
 	<param name="reference_path" value="$(find asift_matching)/reference_data/"/>
 	<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>

rviz_interface/launch/interface_plannar_seg.launch

@@ -2,9 +2,11 @@
 	<node name="rviz" pkg="rviz" type="rviz"/>
 	
 	<!--Interface/-->
-	<rosparam param="object_names"> ["6DOF"]</rosparam>
+	<!--NE PAS CHANGER SANS MODIFIER LE CODE PCL/-->
+	<rosparam param="object_names"> ["foo"]</rosparam>
 	<rosparam param="object_types"> [0]</rosparam>
 	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
+	<param name="object_center_topic" value="/object_center"/>
 
 	
 	<node name="plannar_seg" pkg="pcl_tests" type="plannar_seg"/>

rviz_interface/launch/interface_plannar_seg.launch~

@@ -2,9 +2,10 @@
 	<node name="rviz" pkg="rviz" type="rviz"/>
 	
 	<!--Interface/-->
-	<rosparam param="tracked_object_names"> ["6DOF"]</rosparam>
+	<rosparam param="object_names"> ["foo"]</rosparam>
-	<rosparam param="tracked_object_types"> [0]</rosparam>
+	<rosparam param="object_types"> [0]</rosparam>
 	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
+	<param name="object_center_topic" value="/object_center"/>
 
 	
 	<node name="plannar_seg" pkg="pcl_tests" type="plannar_seg"/>

rviz_interface/msg/StateSpace.msg

@@ -11,7 +11,7 @@ string name
 # ORDINARY : General display / compute of the state space without any assumption.
 # STATE_2D : Expect a 2D position and orientation (Dimension = 3).
 # STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7).
-# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1).
+# STATE_BOOL : Expect a single boolean information (ie on/off button) (Dimension = 1).
 uint8 ORDINARY=0
 uint8 STATE_BOOL=1
 uint8 STATE_2D=2
@@ -19,7 +19,7 @@ uint8 STATE_3D=3
 
 uint8 state_type
 
-#Objective type: boolean (true : Juste bouger l'objet)
+#Objective type: boolean (true : Just interact with the object without taking in account State Space data)
 uint8 objective_type
 
 #Maximum distance error allowed arround the objective

rviz_interface/msg/StateSpace.msg~

@@ -11,15 +11,15 @@ string name
 # ORDINARY : General display / compute of the state space without any assumption.
 # STATE_2D : Expect a 2D position and orientation (Dimension = 3).
 # STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7).
-# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1).
+# STATE_BOOL : Expect a single boolean information (ie on/off button) (Dimension = 1).
-uint8 ORDINARY=0 
+uint8 ORDINARY=0
-uint8 STATE_2D=1
+uint8 STATE_BOOL=1
-uint8 STATE_3D=2
+uint8 STATE_2D=2
-uint8 STATE_BOOL=3
+uint8 STATE_3D=3
 
 uint8 state_type
 
-#Objective type: boolean (true : Juste bouger l'objet)
+#Objective type: boolean (true : Just move the object without taking in account State Space data)
 uint8 objective_type
 
 #Maximum distance error allowed arround the objective

rviz_interface/src/InteractiveObject.cpp

@@ -9,7 +9,7 @@
 unsigned int InteractiveObject::nextObjectID = 1;
 
 //Constructeur
-InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape = Marker::CUBE, const tf::Vector3& position) : _name(name), _type(type), _server(server), _showVisuals(true), _followObject(true)
+InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, const std::string& frame_id, unsigned int type, unsigned int shape = Marker::CUBE, const tf::Vector3& position) : _name(name), _type(type), _server(server), _showVisuals(true), _followObject(true)
 {
 	_objectID = nextObjectID++;
 	_state.name = name;
@@ -26,16 +26,16 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe
 	marker.color.b = 0.5;
 	marker.color.a = 1.0;
 
-	createInteractiveMarker(marker, position);
+	createInteractiveMarker(marker, frame_id, position);
 	addButtoncontrol();
 }
 
  //Construit un InteractiveMarker
-void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector3& position)
+void InteractiveObject::createInteractiveMarker(Marker& marker, const std::string& frame_id, const tf::Vector3& position)
 {
 	//// Création d'un marker interactif ////
 	// _int_marker.header.frame_id = "/map"; //Par défaut
-	_int_marker.header.frame_id = "/camera_rgb_optical_frame";
+	_int_marker.header.frame_id = frame_id;
 	_int_marker.header.stamp= ros::Time::now();
 	_int_marker.name = _name; //ATTENTION !
 	_int_marker.description = _name;

rviz_interface/src/InteractiveObject.hpp

@@ -36,10 +36,10 @@ protected:
 	ros::Publisher* _visual_pub;
 
 public:
-	InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape, const tf::Vector3& position = tf::Vector3(0,0,0));
+	InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, const std::string& frame_id, unsigned int type, unsigned int shape, const tf::Vector3& position = tf::Vector3(0,0,0));
 
 	//Construit un InteractiveMarker
-	void createInteractiveMarker(Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0));
+	void createInteractiveMarker(Marker& marker, const std::string& frame_id, const tf::Vector3& position = tf::Vector3(0,0,0));
 
 	//Fonction callback du serveur d' InteractiveMarker
 	void processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback );

rviz_interface/src/RvizInterface.cpp

@@ -54,19 +54,19 @@ RvizInterface::RvizInterface(): _server("RvizInterface")
 		switch(object_types[i])
 		{
 			case 0:
-				_objects.push_back(new InteractiveObject(&_server, object_names[i], (int) rviz_interface::StateSpace::ORDINARY, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
+				_objects.push_back(new InteractiveObject(&_server, object_names[i], frame_id, (int) rviz_interface::StateSpace::ORDINARY, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 				_objects[i]->add6DOFcontrol();
 				break;
 			case 1:
-				_objects.push_back(new InteractiveObject(&_server, object_names[i], (int) rviz_interface::StateSpace::STATE_BOOL, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
+				_objects.push_back(new InteractiveObject(&_server, object_names[i], frame_id, (int) rviz_interface::StateSpace::STATE_BOOL, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 				// _objects[i]->addButtoncontrol();
 				break;
 			case 2:
-				_objects.push_back(new InteractiveObject(&_server, object_names[i], (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
+				_objects.push_back(new InteractiveObject(&_server, object_names[i], frame_id, (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 				_objects[i]->add3DOFcontrol();
 				break;
 			case 3:
-				_objects.push_back(new InteractiveObject(&_server, object_names[i], (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
+				_objects.push_back(new InteractiveObject(&_server, object_names[i], frame_id, (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 				_objects[i]->add6DOFcontrol();
 				break;
 		}