Fin ajout zone d'erreur
This commit is contained in:
Unknown
parent
2674dc7da2
commit
e9ee595fb5
7 changed files with 100 additions and 73 deletions
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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; 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
|
||||
// }
|
||||
// }
|
||||
_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);
|
||||
}
|
||||
}
|
||||
|
|
@ -20,12 +20,13 @@ protected:
|
|||
//std::vector<visualization_msgs::InteractiveMarker> _int_markers;
|
||||
InteractiveMarker _int_marker;
|
||||
// std::vector<Marker> _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<visualization_msgs::InteractiveMarker>& 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
|
||||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -4,25 +4,23 @@ 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);
|
||||
_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);
|
||||
_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
|
||||
{
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue