From e9ee595fb5286640c7e3b65b6369fd484944d8db Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 29 Jun 2018 18:22:49 +0200 Subject: [PATCH] Fin ajout zone d'erreur --- .../rviz_interface/msg/InterfaceConfig.msg | 3 + .../rviz_interface/msg/InterfaceConfig.msg~ | 5 + .../rviz_interface/src/InteractiveObject.cpp | 113 +++++++++--------- .../rviz_interface/src/InteractiveObject.hpp | 13 +- .../rviz_interface/src/InterfacePanel.cpp | 22 +++- .../rviz_interface/src/InterfacePanel.hpp | 2 + .../rviz_interface/src/RvizInterface.cpp | 15 ++- 7 files changed, 100 insertions(+), 73 deletions(-) diff --git a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg index 840a232..a10e6dd 100644 --- a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg +++ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg @@ -12,3 +12,6 @@ float32 max_error #Follow Object: boolean (true : les marqueur suivent les objets) uint8 follow_object +#Show Visuals: Affichage d'infos supplémentaires (zone d'erreur) +uint8 show_visuals + diff --git a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ index 8753ee3..9030c53 100644 --- a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ +++ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ @@ -9,3 +9,8 @@ uint8 objective_type #Maximum distance error allowed arround the objective float32 max_error +#Follow Object: boolean (true : les marqueur suivent les objets) +uint8 follow_object + +#Show Visuals: Affichage d'infos supplémentaires (zone d'erreur) + diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp index 43e413f..425b48f 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.cpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.cpp @@ -27,7 +27,7 @@ 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"; + _int_marker.header.frame_id = "/map"; //Par défaut _int_marker.header.stamp=ros::Time::now(); _int_marker.name = _name; //ATTENTION ! _int_marker.description = _name; @@ -55,30 +55,12 @@ void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr { _followObject = false; //Objet manipulé -> on les libèrent - //// 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 - // } - // } + _int_marker.pose = feedback->pose; //Màj du marqueur interne if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK ) { // Send objective - if(_objective_pub && feedback->control_name == "control") + if(ros::ok() && _objective_pub && feedback->control_name == "control") { rviz_interface::StateSpace msg; msg.name = _state.name; @@ -108,52 +90,35 @@ void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr //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; - // } } + updateVisuals(); } -//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(); + _error_marker.header.frame_id = _int_marker.header.frame_id; + _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; + // 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 = _int_marker.scale; + _error_marker.scale.y = _int_marker.scale; + _error_marker.scale.z = _int_marker.scale; + _error_marker.color.r = 1; + _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(); + //Publie le marker si possible + if( ros::ok() && _visual_pub ) + { + _visual_pub->publish(_error_marker); + } } void InteractiveObject::addButtoncontrol() @@ -280,6 +245,8 @@ void InteractiveObject::setErrorArea(double error) _showVisuals = true; _state.max_error = error; + + updateVisuals(); } void InteractiveObject::moveTo(const tf::Vector3& new_pos) @@ -287,4 +254,32 @@ void InteractiveObject::moveTo(const tf::Vector3& new_pos) tf::pointTFToMsg(new_pos, _int_marker.pose.position); _server->insert(_int_marker); _server->applyChanges(); + + updateVisuals(); +} + +void InteractiveObject::updateVisuals() +{ + // ROS_INFO_STREAM("Update visuals"); + if(!_showVisuals) + { + // ROS_INFO_STREAM("Suppresion des visuels"); + _error_marker.action = 2; //Suppresion du marker + } + else + { + _error_marker.action = 0; //Ajout/modification du marker + + _error_marker.scale.x = _state.max_error; + _error_marker.scale.y = _state.max_error; + _error_marker.scale.z = _state.max_error; + + _error_marker.pose = _int_marker.pose; + } + + //Publie le marker si possible + if( ros::ok() && _visual_pub ) + { + _visual_pub->publish(_error_marker); + } } \ 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 032b401..9ebd7c9 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.hpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.hpp @@ -20,12 +20,13 @@ protected: //std::vector _int_markers; InteractiveMarker _int_marker; // std::vector _visual_markers; - InteractiveMarkerControl visual_container; + Marker _error_marker; interactive_markers::InteractiveMarkerServer* _server; rviz_interface::StateSpace _state; ros::Publisher* _objective_pub; + ros::Publisher* _visual_pub; public: InteractiveObject(interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape, const tf::Vector3& position); @@ -39,18 +40,20 @@ public: void add3DOFcontrol(); void addVisuals(); + void updateVisuals(); void setObjectivePublisher(ros::Publisher* objective_pub){ _objective_pub=objective_pub;} - // void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visualization_pub=visualization_pub;} + void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visual_pub=visualization_pub;} void setErrorArea(double error); void moveTo(const tf::Vector3& new_pos); - //std::vector& markers(){ return _int_markers;} - InteractiveMarker& marker(){ return _int_marker;} + InteractiveMarker& int_marker(){ return _int_marker;} + Marker& err_marker(){ return _error_marker;} rviz_interface::StateSpace& state(){ return _state;} bool isFollowing() const{ return _followObject;} - void follow(){ _followObject = true;} + void follow(bool follow = true){ _followObject = follow;} + void showVisuals(bool showVisuals = true){ _showVisuals=showVisuals; updateVisuals();} }; #endif \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/InterfacePanel.cpp b/rviz_interface/rviz_interface/src/InterfacePanel.cpp index d698919..fd1e2f3 100644 --- a/rviz_interface/rviz_interface/src/InterfacePanel.cpp +++ b/rviz_interface/rviz_interface/src/InterfacePanel.cpp @@ -32,11 +32,13 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) // Lay out the topic field above the control widget. QVBoxLayout* layout = new QVBoxLayout; _objective_type_editor = new QCheckBox( "Precise Objective" ); + _show_visuals_editor = new QCheckBox( "Show Visuals" ); _reset_button = new QPushButton("Reset", this); layout->addWidget( _reset_button ); layout->addWidget( _objective_type_editor ); layout->addLayout( error_layout ); + layout->addWidget( _show_visuals_editor ); setLayout( layout ); // Create a timer for sending the output. Motor controllers want to @@ -54,6 +56,7 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) connect( _max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() )); connect( _objective_type_editor, SIGNAL( stateChanged(int) ), this, SLOT( updateType(int) )); connect(_reset_button, SIGNAL (released()), this, SLOT (handleResetButton())); + connect( _show_visuals_editor, SIGNAL( stateChanged(int) ), this, SLOT( updateVisuals(int) )); // connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); // Start the timer. @@ -89,11 +92,28 @@ void InterfacePanel::updateError() void InterfacePanel::updateType(int state) { current_config.objective_type = state; + + _max_error_editor->setEnabled( state ); //Active l'editeur d'erreur si state>0 (ie Objectif précis) + if(! state) //Pas d'objectif précis + { + _show_visuals_editor->setCheckState( Qt::Unchecked ); //Affichage de la zone d'erreur inutile + current_config.show_visuals = 0; + } + + //Publication si possible + if( ros::ok() && _config_publisher ) + { + _config_publisher.publish( current_config ); + } +} + +void InterfacePanel::updateVisuals(int state) +{ + current_config.show_visuals = state; if( ros::ok() && _config_publisher ) { _config_publisher.publish( current_config ); } - _max_error_editor->setEnabled( state ); //Active l'editeur d'erreur si state>0 (ie Objectif précis) } void InterfacePanel::handleResetButton() diff --git a/rviz_interface/rviz_interface/src/InterfacePanel.hpp b/rviz_interface/rviz_interface/src/InterfacePanel.hpp index f39fa2c..9a5b91f 100644 --- a/rviz_interface/rviz_interface/src/InterfacePanel.hpp +++ b/rviz_interface/rviz_interface/src/InterfacePanel.hpp @@ -65,6 +65,7 @@ protected Q_SLOTS: void updateError(); void updateType(int state); + void updateVisuals(int state); void handleResetButton(); @@ -76,6 +77,7 @@ protected: // One-line text editor for entering the outgoing ROS topic name. QLineEdit* _max_error_editor; QCheckBox* _objective_type_editor; + QCheckBox* _show_visuals_editor; QPushButton* _reset_button; // The current name of the output topic. diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp index bf23ad9..401d31b 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.cpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -4,25 +4,23 @@ 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); + _visualization_pub = _n.advertise("/RvizInterface/visual_marker", 10); //Topic you want to subscribe _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[0].addButtonControl(); _objects[0]->add6DOFcontrol(); // _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(); + _objects[i]->setVisualizationPublisher(&_visualization_pub); + _objects[i]->addVisuals(); } } @@ -41,10 +39,10 @@ void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_c _objects[i]->state().objective_type = new_config.objective_type; // _objects[i]->state().max_error = new_config.max_error; _objects[i]->setErrorArea(new_config.max_error); - + _objects[i]->showVisuals(new_config.show_visuals); if(new_config.follow_object && !_objects[i]->isFollowing()) //Signal reset + pas de suivie { - _objects[i]->follow(); //Le ùarquer suit de nouveau l'objet + _objects[i]->follow(); //Le marker suit de nouveau l'objet } } } @@ -53,7 +51,8 @@ void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_c void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center) { //Synchro des frames - _objects[0]->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; if(_objects[0]->isFollowing()) //Le marqueur suit l'objet {