AntoineH/BaxterInterface
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Essaie ajout zone d'erreur

Browse source 
Non concluent -> a resoudre
This commit is contained in:
Unknown 2018-06-28 14:40:20 +02:00
parent 24eb9f7713
commit 63218c9c06
4 changed files with 140 additions and 47 deletions
Download patch file Download diff file Expand all files Collapse all files

158
rviz_interface/rviz_interface/src/InteractiveObject.cpp
View file

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

12
rviz_interface/rviz_interface/src/InteractiveObject.hpp
View file

@ -15,8 +15,11 @@ protected:
static unsigned int nextObjectID;
std::string _name;
unsigned int _type;
bool _showVisuals;
//std::vector<visualization_msgs::InteractiveMarker> _int_markers;
InteractiveMarker _int_marker;
// std::vector<Marker> _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<visualization_msgs::InteractiveMarker>& markers(){ return _int_markers;}
InteractiveMarker& marker(){ return _int_marker;}
rviz_interface::StateSpace& state(){ return _state;}

15
rviz_interface/rviz_interface/src/RvizInterface.cpp
View file

@ -4,17 +4,25 @@ 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);
//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);
}
}

2
rviz_interface/rviz_interface/src/RvizInterface.hpp
View file

@ -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<InteractiveObject*> _objects;