From 63218c9c069f8b75b61c7c9337a733beb7838f44 Mon Sep 17 00:00:00 2001 From: Unknown Date: Thu, 28 Jun 2018 14:40:20 +0200 Subject: [PATCH] Essaie ajout zone d'erreur Non concluent -> a resoudre --- .../rviz_interface/src/InteractiveObject.cpp | 158 +++++++++++++----- .../rviz_interface/src/InteractiveObject.hpp | 12 +- .../rviz_interface/src/RvizInterface.cpp | 15 +- .../rviz_interface/src/RvizInterface.hpp | 2 + 4 files changed, 140 insertions(+), 47 deletions(-) diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp index c0052b5..7603785 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.cpp +++ b/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; - container.always_visible = true; //Toujours visible hors du mode interaction - container.markers.push_back( marker ); + InteractiveMarkerControl interactive_container; + interactive_container.name = "control"; + 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,41 +51,106 @@ 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; ipose; + // } + // //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 //// - if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK) - { - 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++) + // Send objective + if(_objective_pub && feedback->control_name == "control") { - 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]); - } + 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; - //Publication de l'objectif - _objective_pub->publish(msg); + //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); - //Problème d'ajout continue de data - //_state.real_data.clear(); //Attention peut poser problème pour ajouter des infos supplémentaires (nécessité de les rajouter à chaque feedback) - } + //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]); + } + 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() @@ -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; } \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.hpp b/rviz_interface/rviz_interface/src/InteractiveObject.hpp index 6fc10cb..a389386 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.hpp +++ b/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 _int_markers; InteractiveMarker _int_marker; + // std::vector _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& markers(){ return _int_markers;} InteractiveMarker& marker(){ return _int_marker;} rviz_interface::StateSpace& state(){ return _state;} diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp index 29cb0f6..fafbfc8 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.cpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -4,17 +4,25 @@ RvizInterface::RvizInterface(): _server("RvizInterface") { //Topic you want to publish _objective_pub = _n.advertise("/RvizInterface/state_objective", 10); + // _visualization_pub = _n.advertise("/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); } } diff --git a/rviz_interface/rviz_interface/src/RvizInterface.hpp b/rviz_interface/rviz_interface/src/RvizInterface.hpp index 3c764b2..d28f739 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.hpp +++ b/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 _objects;