Essaie ajout zone d'erreur

Non concluent -> a resoudre

rviz_interface/rviz_interface/src/InteractiveObject.cpp

@@ -2,7 +2,7 @@
 
 unsigned int InteractiveObject::nextObjectID = 1;
 
-InteractiveObject::InteractiveObject(ros::Publisher* objective_pub, interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape = Marker::CUBE, const tf::Vector3& position = tf::Vector3(0,0,0)) : _name(name), _type(type), _server(server), _objective_pub(objective_pub)
+InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape = Marker::CUBE, const tf::Vector3& position = tf::Vector3(0,0,0)) : _name(name), _type(type), _server(server), _showVisuals(true)
 {
 	_objectID = nextObjectID++;
 	_state.name = name;
@@ -21,26 +21,26 @@ InteractiveObject::InteractiveObject(ros::Publisher* objective_pub, interactive_
 
 	createInteractiveMarker(marker, position);
 	addButtoncontrol();
-	// add6DOFcontrol();
 }
 
  //Name must be unique
 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";
+	_int_marker.header.frame_id = "/map";
 	_int_marker.header.stamp=ros::Time::now();
 	_int_marker.name = _name; //ATTENTION !
 	_int_marker.description = _name;
 	tf::pointTFToMsg(position, _int_marker.pose.position);
 
 	// create a non-interactive control which contains the box
-	InteractiveMarkerControl container;
+	InteractiveMarkerControl interactive_container;
-	container.always_visible = true; //Toujours visible hors du mode interaction
+	interactive_container.name = "control";
-	container.markers.push_back( marker );
+	interactive_container.always_visible = true; //Toujours visible hors du mode interaction
+	interactive_container.markers.push_back( marker );
 
 	// add the control to the interactive marker
-	_int_marker.controls.push_back( container );
+	_int_marker.controls.push_back( interactive_container );
 
 	// add the interactive marker to our collection &
 	// tell the server to call processFeedback() when feedback arrives for it
@@ -51,43 +51,108 @@ void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector
 }
 
 void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback )
+{
+	//// Update Visual markers ////
+	// if(_showVisuals)
+	// {
+	// 	//Màj des visuels
+	// 	for(unsigned int i=0; i<visual_container.markers.size();i++)
+	// 	{
+	// 		visual_container.markers[i].pose = feedback->pose;
+	// 	}
+	// 	//Ajout du container
+	// 	ROS_INFO_STREAM("Name : "<<_int_marker.controls.back().name<<" / Size : "<<_int_marker.controls.size());
+	// 	if(_int_marker.controls.back().name=="visual") 
+	// 	{
+	// 		_int_marker.controls.back()=visual_container; //Modification du container deja present
+	// 	}
+	// 	else
+	// 	{
+	// 		_int_marker.controls.push_back( visual_container ); //Ajout
+	// 	}
+	// }
+
+	if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK )
 	{
-	//// SEND OBJECTIVE ////
+		// Send objective
-	if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK)
+		if(_objective_pub && feedback->control_name == "control")
-	{
-		rviz_interface::StateSpace msg;
-		msg.name = _state.name;
-		msg.state_type = _state.state_type;
-		msg.objective_type = _state.objective_type;
-		msg.max_error = _state.max_error;
-
-		//Ajout des informations de position
-		msg.real_data.push_back(feedback->pose.position.x);
-		msg.real_data.push_back(feedback->pose.position.y);
-		msg.real_data.push_back(feedback->pose.position.z);
-		msg.real_data.push_back(feedback->pose.orientation.w);
-		msg.real_data.push_back(feedback->pose.orientation.x);
-		msg.real_data.push_back(feedback->pose.orientation.y);
-		msg.real_data.push_back(feedback->pose.orientation.z);
-
-		//Ajout des inforamtions supplémentaires
-		for(unsigned int i=0;i<_state.real_data.size();i++)
 		{
-			msg.real_data.push_back(_state.real_data[i]);
+			rviz_interface::StateSpace msg;
-		}
+			msg.name = _state.name;
-		for(unsigned int i=0;i<_state.discrete_data.size();i++)
+			msg.state_type = _state.state_type;
-		{
+			msg.objective_type = _state.objective_type;
-			msg.discrete_data.push_back(_state.discrete_data[i]);
+			msg.max_error = _state.max_error;
-		}
 
-		//Publication de l'objectif
+			//Ajout des informations de position
-		_objective_pub->publish(msg);
+			msg.real_data.push_back(feedback->pose.position.x);
+			msg.real_data.push_back(feedback->pose.position.y);
+			msg.real_data.push_back(feedback->pose.position.z);
+			msg.real_data.push_back(feedback->pose.orientation.w);
+			msg.real_data.push_back(feedback->pose.orientation.x);
+			msg.real_data.push_back(feedback->pose.orientation.y);
+			msg.real_data.push_back(feedback->pose.orientation.z);
 
-		//Problème d'ajout continue de data
+			//Ajout des inforamtions supplémentaires
-		//_state.real_data.clear(); //Attention peut poser problème pour ajouter des infos supplémentaires (nécessité de les rajouter à chaque feedback)
+			for(unsigned int i=0;i<_state.real_data.size();i++)
+			{
+				msg.real_data.push_back(_state.real_data[i]);
+			}
+			for(unsigned int i=0;i<_state.discrete_data.size();i++)
+			{
+				msg.discrete_data.push_back(_state.discrete_data[i]);
+			}
+
+			//Publication de l'objectif
+			_objective_pub->publish(msg);
+		}
+		// Hide visuals
+		// if(feedback->control_name == "visual")
+		// {
+		// 	_int_marker.controls.pop_back();
+		// 	_server->insert(_int_marker);
+		// 	_server->applyChanges();
+		// 	_showVisuals = false;
+		// }
 	}	
 }
 
+//Attention a faire en dernier ! visual container doit etre le dernier control
+void InteractiveObject::addVisuals()
+{
+	//Create the error area
+	// InteractiveMarkerControl visual_container;
+	visual_container.name = "visual";
+	visual_container.interaction_mode = InteractiveMarkerControl::BUTTON; //Click on visual effect to make them disappear
+
+	Marker error_marker;
+	// Set the frame ID and timestamp.  See the TF tutorials for information on these.
+    // error_marker.header.frame_id = "/map";
+    // error_marker.header.stamp = ros::Time::now();
+
+    // // Set the namespace and id for this marker.  This serves to create a unique ID
+    // // Any marker sent with the same namespace and id will overwrite the old one
+    // error_marker.ns = _name;
+    // error_marker.id = 0;
+	error_marker.type = visualization_msgs::Marker::SPHERE;
+	error_marker.scale.x = 1;
+	error_marker.scale.y = 1;
+	error_marker.scale.z = 1;
+	error_marker.color.r = 0.5;
+	error_marker.color.g = 0.5;
+	error_marker.color.b = 0.5;
+	error_marker.color.a = 0.5;
+	error_marker.pose = _int_marker.pose;
+
+	// _visual_markers.push_back(error_marker);
+
+	visual_container.markers.push_back( error_marker );
+
+	_int_marker.controls.push_back( visual_container );
+
+	_server->insert(_int_marker);
+	_server->applyChanges();
+}
+
 void InteractiveObject::addButtoncontrol()
 {
 	//// Ajout d'interactions ////
@@ -98,7 +163,7 @@ void InteractiveObject::addButtoncontrol()
 
 	// add the interactive marker to our collection &
 	// tell the server to call processFeedback() when feedback arrives for it
-	_server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
+	_server->insert(_int_marker);
 
 	// 'commit' changes and send to all clients
 	_server->applyChanges();
@@ -110,7 +175,7 @@ void InteractiveObject::add6DOFcontrol()
 	//// Ajout d'interactions ////
 	InteractiveMarkerControl marker_control;
 
-	_int_marker.controls[0].name= "3D";
+	//_int_marker.controls[0].name= "3D";
 
 	marker_control.orientation_mode = InteractiveMarkerControl::FIXED;
 	//Orientation du vecteur
@@ -154,7 +219,7 @@ void InteractiveObject::add6DOFcontrol()
 
 	// add the interactive marker to our collection &
 	// tell the server to call processFeedback() when feedback arrives for it
-	_server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
+	_server->insert(_int_marker);
 
 	// 'commit' changes and send to all clients
 	_server->applyChanges();
@@ -166,7 +231,7 @@ void InteractiveObject::add3DOFcontrol()
 	//// Ajout d'interactions ////
 	InteractiveMarkerControl marker_control;
 
-	_int_marker.controls[0].name= "2D";
+	//_int_marker.controls[0].name= "2D";
 
 	marker_control.orientation_mode = InteractiveMarkerControl::FIXED;
 	//Orientation du vecteur
@@ -201,8 +266,15 @@ void InteractiveObject::add3DOFcontrol()
 
 	// add the interactive marker to our collection &
 	// tell the server to call processFeedback() when feedback arrives for it
-	_server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
+	_server->insert(_int_marker);
 
 	// 'commit' changes and send to all clients
 	_server->applyChanges();
 }
+
+void InteractiveObject::setErrorArea(double error)
+{
+	_showVisuals = true;
+
+	_state.max_error = error;
+}

rviz_interface/rviz_interface/src/InteractiveObject.hpp

@@ -15,8 +15,11 @@ protected:
 	static unsigned int nextObjectID;
 	std::string _name;
 	unsigned int _type;
+	bool _showVisuals;
 	//std::vector<visualization_msgs::InteractiveMarker> _int_markers;
 	InteractiveMarker _int_marker;
+	// std::vector<Marker> _visual_markers;
+	InteractiveMarkerControl visual_container;
 
 	interactive_markers::InteractiveMarkerServer* _server;
 	rviz_interface::StateSpace _state;
@@ -24,7 +27,7 @@ protected:
 	ros::Publisher* _objective_pub;
 
 public:
-	InteractiveObject(ros::Publisher* objective_pub, interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape, const tf::Vector3& position);
+	InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape, const tf::Vector3& position);
 
 	void createInteractiveMarker(Marker& marker, const tf::Vector3& position);
 
@@ -34,6 +37,13 @@ public:
 	void add6DOFcontrol();
 	void add3DOFcontrol();
 
+	void addVisuals();
+
+	void setObjectivePublisher(ros::Publisher* objective_pub){ _objective_pub=objective_pub;}
+	// void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visualization_pub=visualization_pub;}
+	void setErrorArea(double error);
+
+
 	//std::vector<visualization_msgs::InteractiveMarker>& markers(){ return _int_markers;}
 	InteractiveMarker& marker(){ return _int_marker;}
 	rviz_interface::StateSpace& state(){ return _state;}

rviz_interface/rviz_interface/src/RvizInterface.cpp

@@ -4,17 +4,25 @@ RvizInterface::RvizInterface(): _server("RvizInterface")
 {
 	//Topic you want to publish
 	_objective_pub = _n.advertise<rviz_interface::StateSpace>("/RvizInterface/state_objective", 10);
+	// _visualization_pub = _n.advertise<visualization_msgs::Marker>("/RvizInterface/visual_marker", 10);
 
 	//Topic you want to subscribe
 	_config_sub = _n.subscribe("/RvizInterface/interface_config", 1, &RvizInterface::configCallback, this);
 
-	_objects.push_back(new InteractiveObject(&_objective_pub, &_server, "Test", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(0,0,0)));
+	_objects.push_back(new InteractiveObject(&_server, "Test", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(0,0,0)));
 	// _objects[0].addButtonControl();
 	_objects[0]->add6DOFcontrol();
 
-	_objects.push_back(new InteractiveObject(&_objective_pub, &_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, "Test 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(3,3,0)));
 	// _objects[0].addButtonControl();
 	_objects[1]->add3DOFcontrol();
+
+	for(unsigned int i=0;i<_objects.size();i++)
+	{
+		_objects[i]->setObjectivePublisher(&_objective_pub);
+		// _objects[i]->setVisualizationPublisher(&_visualization_pub);
+		// _objects[i]->addVisuals();
+	}
 }
 
 RvizInterface::~RvizInterface()
@@ -30,7 +38,8 @@ void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_c
 	for(unsigned int i=0;i<_objects.size();i++)
 	{
 		_objects[i]->state().objective_type = new_config.objective_type;
-		_objects[i]->state().max_error = new_config.max_error;
+		// _objects[i]->state().max_error = new_config.max_error;
+		_objects[i]->setErrorArea(new_config.max_error);
 	}
 }
 

rviz_interface/rviz_interface/src/RvizInterface.hpp

@@ -15,7 +15,9 @@ class RvizInterface
 protected:
 	ros::NodeHandle _n; 
 	ros::Publisher _objective_pub;
+	ros::Publisher _visualization_pub;
 	ros::Subscriber _config_sub;
+	
 	interactive_markers::InteractiveMarkerServer _server;
 
 	std::vector<InteractiveObject*> _objects;