Ajout de la checkBox a l'interface Panel

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
 

rviz_interface/rviz_interface/msg/InterfaceConfig.msg~

@@ -3,33 +3,9 @@
 # General info.
 #Header header
 
-# Identifying string for this state space. (ID ?)
+#Objective type: boolean (true : Juste bouger l'objet)
-string name
+uint8 objective_type
-
-# 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
 
 #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

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.
+#Objective type: boolean (true : Juste bouger l'objet)
-#uint8 dimension
+uint8 objective_type
 
 #Maximum distance error allowed arround the objective
 float32 max_error

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.
+#Objective type: boolean (true : Juste bouger l'objet)
-#uint8 dimension
+uint8 objective_type
+
+#Maximum distance error allowed arround the objective
+float32 max_error
 
 #State Space data : contains the state space data.
 uint8[] discrete_data

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);
+		msg.real_data.push_back(feedback->pose.position.x);
-		_state.real_data.push_back(feedback->pose.position.y);
+		msg.real_data.push_back(feedback->pose.position.y);
-		_state.real_data.push_back(feedback->pose.position.z);
+		msg.real_data.push_back(feedback->pose.position.z);
-		_state.real_data.push_back(feedback->pose.orientation.w);
+		msg.real_data.push_back(feedback->pose.orientation.w);
-		_state.real_data.push_back(feedback->pose.orientation.x);
+		msg.real_data.push_back(feedback->pose.orientation.x);
-		_state.real_data.push_back(feedback->pose.orientation.y);
+		msg.real_data.push_back(feedback->pose.orientation.y);
-		_state.real_data.push_back(feedback->pose.orientation.z);
+		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)
 	}
 }
 

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;
+  QHBoxLayout* error_layout = new QHBoxLayout;
-  topic_layout->addWidget( new QLabel( "Max Error:" ));
+  error_layout->addWidget( new QLabel( "Max Error:" ));
-  max_error_editor = new QLineEdit;
+  _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 );
+  error_layout->addWidget( _max_error_editor );
-  setLayout( topic_layout );
+  //setLayout( error_layout );
 
   // Then create the control widget.
   // drive_widget_ = new DriveWidget;
 
-  // // Lay out the topic field above the control widget.
+  // Lay out the topic field above the control widget.
-  // QVBoxLayout* layout = new QVBoxLayout;
+  QVBoxLayout* layout = new QVBoxLayout;
-  // layout->addLayout( topic_layout );
+  _objective_type_editor = new QCheckBox( "Precise Objective" );
-  // layout->addWidget( drive_widget_ );
+  layout->addWidget( _objective_type_editor );
-  // setLayout( layout );
+  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<rviz_interface::InterfaceConfig>( "/RvizInterface/interface_config", 1 );
+  _config_publisher = _nh.advertise<rviz_interface::InterfaceConfig>( "/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() );
+  current_config.max_error = _max_error_editor->text().toDouble();
-  setError(max_error_editor->text());
-}
 
-void InterfacePanel::setError( const QString& error )
-{
   if( ros::ok() && _config_publisher )
   {
-    rviz_interface::InterfaceConfig new_config;
+    _config_publisher.publish( current_config );
-    new_config.max_error = error.toDouble();
-    _config_publisher.publish( new_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();
   }
 }

rviz_interface/rviz_interface/src/InterfacePanel.hpp

@@ -12,6 +12,7 @@
 #include <QPainter>
 #include <QLineEdit>
 // #include <QDoubleValidator>
+#include <QCheckBox>
 #include <QVBoxLayout>
 #include <QHBoxLayout>
 #include <QLabel>
@@ -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_;

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;
 	}
 }