diff --git a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg index d91f75e..8753ee3 100644 --- a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg +++ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg @@ -3,6 +3,9 @@ # General info. #Header header +#Objective type: boolean (true : Juste bouger l'objet) +uint8 objective_type + #Maximum distance error allowed arround the objective float32 max_error diff --git a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ index 5ae3257..8753ee3 100644 --- a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ +++ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ @@ -3,33 +3,9 @@ # General info. #Header header -# Identifying string for this state space. (ID ?) -string name - -# State Space type: controls how the state space is displayed / computed. -# Default: ORDINARY -# 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). -uint8 ORDINARY=0 -uint8 STATE_2D=1 -uint8 STATE_3D=2 -uint8 STATE_BOOL=3 - -uint8 state_type - -#State Space dimension : must be equal to the sum of all the data dimensions. -#uint8 dimension +#Objective type: boolean (true : Juste bouger l'objet) +uint8 objective_type #Maximum distance error allowed arround the objective float32 max_error -#State Space data : contains the state space data. -uint8[] discrete_data -float32[] real_data - -# Short description (< 40 characters) of what this state space represent, -# e.g. "Button A pressed". -# Default: ? -string description diff --git a/rviz_interface/rviz_interface/msg/StateSpace.msg b/rviz_interface/rviz_interface/msg/StateSpace.msg index b848f1b..cb8dca5 100644 --- a/rviz_interface/rviz_interface/msg/StateSpace.msg +++ b/rviz_interface/rviz_interface/msg/StateSpace.msg @@ -19,8 +19,8 @@ uint8 STATE_BOOL=3 uint8 state_type -#State Space dimension : must be equal to the sum of all the data dimensions. -#uint8 dimension +#Objective type: boolean (true : Juste bouger l'objet) +uint8 objective_type #Maximum distance error allowed arround the objective float32 max_error diff --git a/rviz_interface/rviz_interface/msg/StateSpace.msg~ b/rviz_interface/rviz_interface/msg/StateSpace.msg~ index 893409e..cb8dca5 100644 --- a/rviz_interface/rviz_interface/msg/StateSpace.msg~ +++ b/rviz_interface/rviz_interface/msg/StateSpace.msg~ @@ -19,8 +19,11 @@ uint8 STATE_BOOL=3 uint8 state_type -#State Space dimension : must be equal to the sum of all the data dimensions. -#uint8 dimension +#Objective type: boolean (true : Juste bouger l'objet) +uint8 objective_type + +#Maximum distance error allowed arround the objective +float32 max_error #State Space data : contains the state space data. uint8[] discrete_data diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp index c832260..c0052b5 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.cpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.cpp @@ -5,6 +5,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) { _objectID = nextObjectID++; + _state.name = name; _state.state_type=type; _state.max_error = 0; //Default error @@ -54,22 +55,36 @@ void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr //// SEND OBJECTIVE //// if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK) { - _state.name = _name; + 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 - _state.real_data.push_back(feedback->pose.position.x); - _state.real_data.push_back(feedback->pose.position.y); - _state.real_data.push_back(feedback->pose.position.z); - _state.real_data.push_back(feedback->pose.orientation.w); - _state.real_data.push_back(feedback->pose.orientation.x); - _state.real_data.push_back(feedback->pose.orientation.y); - _state.real_data.push_back(feedback->pose.orientation.z); + 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]); + } + 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(_state); + _objective_pub->publish(msg); //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) + //_state.real_data.clear(); //Attention peut poser problème pour ajouter des infos supplémentaires (nécessité de les rajouter à chaque feedback) } } diff --git a/rviz_interface/rviz_interface/src/InterfacePanel.cpp b/rviz_interface/rviz_interface/src/InterfacePanel.cpp index 75daf44..62d08e9 100644 --- a/rviz_interface/rviz_interface/src/InterfacePanel.cpp +++ b/rviz_interface/rviz_interface/src/InterfacePanel.cpp @@ -19,21 +19,22 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) { // Next we lay out the "output topic" text entry field using a // QLabel and a QLineEdit in a QHBoxLayout. - QHBoxLayout* topic_layout = new QHBoxLayout; - topic_layout->addWidget( new QLabel( "Max Error:" )); - max_error_editor = new QLineEdit; + QHBoxLayout* error_layout = new QHBoxLayout; + error_layout->addWidget( new QLabel( "Max Error:" )); + _max_error_editor = new QLineEdit; // max_error_editor->setValidator( new QDoubleValidator(0,MAX_ERROR,1000,max_error_editor)); // Ne gère pas les exceptions tout seul (retourne QValidator::Intermediate pour les valeurs hors bornes) - topic_layout->addWidget( max_error_editor ); - setLayout( topic_layout ); + error_layout->addWidget( _max_error_editor ); + //setLayout( error_layout ); // Then create the control widget. // drive_widget_ = new DriveWidget; - // // Lay out the topic field above the control widget. - // QVBoxLayout* layout = new QVBoxLayout; - // layout->addLayout( topic_layout ); - // layout->addWidget( drive_widget_ ); - // setLayout( layout ); + // Lay out the topic field above the control widget. + QVBoxLayout* layout = new QVBoxLayout; + _objective_type_editor = new QCheckBox( "Precise Objective" ); + layout->addWidget( _objective_type_editor ); + layout->addLayout( error_layout ); + setLayout( layout ); // Create a timer for sending the output. Motor controllers want to // be reassured frequently that they are doing the right thing, so @@ -47,7 +48,8 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) // QTimer* output_timer = new QTimer( this ); // Next we make signal/slot connections. - connect( max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() )); + connect( _max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() )); + connect( _objective_type_editor, SIGNAL( stateChanged(int) ), this, SLOT( updateType(int) )); // connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); // Start the timer. @@ -55,8 +57,15 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) // Make the control widget start disabled, since we don't start with an output topic. // drive_widget_->setEnabled( false ); + _max_error_editor->setEnabled( false ); - _config_publisher = nh_.advertise( "/RvizInterface/interface_config", 1 ); + _config_publisher = _nh.advertise( "/RvizInterface/interface_config", 1 ); +} + +InterfacePanel::~InterfacePanel() +{ + delete _max_error_editor; + delete _objective_type_editor; } // Read the topic name from the QLineEdit and call setTopic() with the @@ -65,27 +74,40 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) // away. void InterfacePanel::updateError() { - //setTopic( max_error_editor->text() ); - setError(max_error_editor->text()); -} + current_config.max_error = _max_error_editor->text().toDouble(); -void InterfacePanel::setError( const QString& error ) -{ if( ros::ok() && _config_publisher ) { - rviz_interface::InterfaceConfig new_config; - new_config.max_error = error.toDouble(); - _config_publisher.publish( new_config ); + _config_publisher.publish( current_config ); } } +void InterfacePanel::updateType(int state) +{ + current_config.objective_type = 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::setError( const QString& error ) +// { +// current_config.max_error = error.toDouble(); +// if( ros::ok() && _config_publisher ) +// { +// _config_publisher.publish( current_config ); +// } +// } + // Save all configuration data from this panel to the given // Config object. It is important here that you call save() // on the parent class so the class id and panel name get saved. void InterfacePanel::save( rviz::Config config ) const { rviz::Panel::save( config ); - config.mapSetValue( "Error", output_topic_ ); + config.mapSetValue( "Error", _max_error ); } // Load all configuration data for this panel from the given Config object. @@ -95,7 +117,7 @@ void InterfacePanel::load( const rviz::Config& config ) QString error; if( config.mapGetString( "Error", &error )) { - max_error_editor->setText( error ); + _max_error_editor->setText( error ); updateError(); } } diff --git a/rviz_interface/rviz_interface/src/InterfacePanel.hpp b/rviz_interface/rviz_interface/src/InterfacePanel.hpp index 855e181..724306a 100644 --- a/rviz_interface/rviz_interface/src/InterfacePanel.hpp +++ b/rviz_interface/rviz_interface/src/InterfacePanel.hpp @@ -12,6 +12,7 @@ #include #include // #include +#include #include #include #include @@ -44,6 +45,7 @@ public: // someone using the class for something else to pass in a parent // widget as they normally would with Qt. InterfacePanel( QWidget* parent = 0 ); + ~InterfacePanel(); // Now we declare overrides of rviz::Panel functions for saving and // loading data from the config file. Here the data is the @@ -54,28 +56,33 @@ public: // Next come a couple of public Qt slots. public Q_SLOTS: - void setError( const QString& error ); + // void setError( const QString& error ); // Here we declare some internal slots. protected Q_SLOTS: void updateError(); + void updateType(int state); + // Then we finish up with protected member variables. protected: + rviz_interface::InterfaceConfig current_config; + // One-line text editor for entering the outgoing ROS topic name. - QLineEdit* max_error_editor; + QLineEdit* _max_error_editor; + QCheckBox* _objective_type_editor; // The current name of the output topic. - QString output_topic_; + QString _max_error; // The ROS publisher for the command velocity. ros::Publisher _config_publisher; // The ROS node handle. - ros::NodeHandle nh_; + ros::NodeHandle _nh; // The latest velocity values from the drive widget. // float linear_velocity_; diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp index 94c189c..29cb0f6 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.cpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -29,6 +29,7 @@ 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; } }