Association marker/objet

Ajout du Message NamedPoint
Synchronisation des frame_id à approfondir

my_pcl_tutorial/src/plannar_segmentation.cpp

@@ -17,6 +17,8 @@
 
 #include <pcl/pcl_config.h>
 
+#include <rviz_interface/NamedPoint.h>
+
 //  void printinfo()
 // {
 //   std::cout<<"OK"<<std::endl;
@@ -88,7 +90,9 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
   Eigen::Vector4f barycentre;
   pcl::compute3DCentroid(*pcl_output, barycentre);
 
-  geometry_msgs::PointStamped center_msg;
+  // geometry_msgs::PointStamped center_msg;
+  rviz_interface::NamedPoint center_msg;
+  center_msg.name = "6DOF";
   // for (size_t i = 0; i < pcl_output->size (); ++i)
   // {
   //   if( pcl_output->points[i].x)
@@ -129,7 +133,8 @@ 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<geometry_msgs::PointStamped> ("/object_center", 1);
+  pubc = nh.advertise<rviz_interface::NamedPoint> ("/object_center", 1);
 
   // printinfo();
   // Spin

rviz_interface/rviz_interface/CMakeLists.txt

@@ -38,6 +38,7 @@ add_message_files(
   FILES
   StateSpace.msg
   InterfaceConfig.msg
+  NamedPoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -58,6 +59,7 @@ add_message_files(
 generate_messages(
   DEPENDENCIES
   std_msgs
+  geometry_msgs
 )
 
 ###################################

rviz_interface/rviz_interface/CMakeLists.txt~

@@ -38,6 +38,7 @@ add_message_files(
   FILES
   StateSpace.msg
   InterfaceConfig.msg
+  NamedPoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -58,6 +59,7 @@ add_message_files(
 generate_messages(
   DEPENDENCIES
   std_msgs
+  geometry_msgs
 )
 
 ###################################

rviz_interface/rviz_interface/msg/NamedPoint.msg

@@ -0,0 +1,11 @@
+#Represents a named point.
+
+# General info.
+Header header
+
+# Name.
+string name
+
+# 3D point.
+geometry_msgs/Point point
+#geometry_msgs/Pose pose

rviz_interface/rviz_interface/msg/NamedPoint.msg~

@@ -0,0 +1,11 @@
+#Represents a named point.
+
+# General info.
+Header header
+
+# Name.
+string name
+
+# 3D point.
+#geometry_msgs/Point point
+geometry_msgs/Pose pose

rviz_interface/rviz_interface/msg/StateSpace.msg

@@ -1,7 +1,7 @@
 # Represents a state space.
 
 # General info.
-#Header header
+Header header
 
 # Identifying string for this state space. (ID ?)
 string name

rviz_interface/rviz_interface/src/InteractiveObject.cpp

@@ -28,8 +28,9 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe
 void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0))
 {
 	//// Création d'un marker interactif ////
-	_int_marker.header.frame_id = "/map"; //Par défaut
+	// _int_marker.header.frame_id = "/map"; //Par défaut
-	_int_marker.header.stamp=ros::Time::now();
+	_int_marker.header.frame_id = "/camera_rgb_optical_frame";
+	_int_marker.header.stamp= ros::Time::now();
 	_int_marker.name = _name; //ATTENTION !
 	_int_marker.description = _name;
 	tf::pointTFToMsg(position, _int_marker.pose.position);

rviz_interface/rviz_interface/src/InteractiveObject.hpp

@@ -59,6 +59,7 @@ public:
 	void moveTo(const tf::Vector3& new_pos);
 
 	//Accesseurs
+	const std::string& name() const{ return _name;}
 	void setObjectivePublisher(ros::Publisher* objective_pub){ _objective_pub=objective_pub;}
 	void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visual_pub=visualization_pub;}
 	InteractiveMarker& int_marker(){ return _int_marker;}

rviz_interface/rviz_interface/src/RvizInterface.cpp

@@ -11,18 +11,27 @@ RvizInterface::RvizInterface(): _server("RvizInterface")
 	_config_sub = _n.subscribe("/RvizInterface/interface_config", 1, &RvizInterface::configCallback, this);
 	_position_sub = _n.subscribe("/object_center", 1, &RvizInterface::positionCallback, this);
 
-	_objects.push_back(new InteractiveObject(&_server, "Test", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
+	_objects.push_back(new InteractiveObject(&_server, "6DOF", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 	_objects[0]->add6DOFcontrol();
-	// _objects[0]->add3DOFcontrol(tf::Vector3(0.5,0.5,0));
 
-	// _objects.push_back(new InteractiveObject(&_server, "Test 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(3,3,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));
+
+	// _objects.push_back(new InteractiveObject(&_server, "Button", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,-1,0)));
 
 	for(unsigned int i=0;i<_objects.size();i++)
 	{
 		_objects[i]->setObjectivePublisher(&_objective_pub);
 		_objects[i]->setVisualizationPublisher(&_visualization_pub);
 		_objects[i]->addVisuals();
+
+		//Synchro des frames
+		// _objects[i]->int_marker().header.frame_id = "/camera_rgb_optical_frame";
+		// _objects[i]->err_marker().header.frame_id = "/camera_rgb_optical_frame";
 	}
 }
 
@@ -53,17 +62,38 @@ void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_c
 
 //Fonction callback gérant la position de l'objet
 //PROVISOIRE : Pour un seul objet avec test PCL
-void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center)
+// void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center)
-{
+// {
-	//Synchro des frames
+// 	//Synchro des frames
-	_objects[0]->int_marker().header.frame_id = new_center.header.frame_id;
+// 	_objects[0]->int_marker().header.frame_id = new_center.header.frame_id;
-	_objects[0]->err_marker().header.frame_id = new_center.header.frame_id;
+// 	_objects[0]->err_marker().header.frame_id = new_center.header.frame_id;
 
-	if(_objects[0]->isFollowing()) //Le marqueur suit l'objet
+// 	if(_objects[0]->isFollowing()) //Le marqueur suit l'objet
+// 	{
+// 		//Deplacement du marqueur
+// 		tf::Vector3 position = tf::Vector3(new_center.point.x,new_center.point.y,new_center.point.z);
+// 		_objects[0]->moveTo(position);
+// 	}
+// }
+
+void RvizInterface::positionCallback(const rviz_interface::NamedPoint & new_center)
+{
+	for(unsigned int i=0;i<_objects.size();i++)
+	{
+		if(_objects[i]->name()==new_center.name)
+		{
+			//Synchro des frames
+			// _objects[i]->int_marker().header.frame_id = new_center.header.frame_id;
+			// _objects[i]->err_marker().header.frame_id = new_center.header.frame_id;
+
+			if(_objects[i]->isFollowing()) //Le marqueur suit l'objet
 			{
 				//Deplacement du marqueur
 				tf::Vector3 position = tf::Vector3(new_center.point.x,new_center.point.y,new_center.point.z);
-		_objects[0]->moveTo(position);
+				_objects[i]->moveTo(position);
+			}
+			break;
+		}
 	}
 }
 

rviz_interface/rviz_interface/src/RvizInterface.hpp

@@ -10,6 +10,8 @@
 
 #include <rviz_interface/InterfaceConfig.h>
 
+#include <rviz_interface/NamedPoint.h>
+
 class RvizInterface
 {
 protected:
@@ -38,7 +40,7 @@ public:
 
 	//Fonction callback gérant la position de l'objet
 	//PROVISOIRE : Pour un seul objet avec test PCL
-	void positionCallback(const geometry_msgs::PointStamped & new_center);
+	void positionCallback(const rviz_interface::NamedPoint & new_center);
 };
 
 #endif