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

Ajout Commentaires

Browse source
This commit is contained in:
Unknown 2018-07-02 11:09:50 +02:00
parent e9ee595fb5
commit cbb0d72367
4 changed files with 78 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2,6 +2,7 @@
unsigned int InteractiveObject::nextObjectID = 1; unsigned int InteractiveObject::nextObjectID = 1;
//Constructeur
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), _followObject(true) 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), _followObject(true)
{ {
_objectID = nextObjectID++; _objectID = nextObjectID++;
@ -11,9 +12,9 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe
Marker marker; Marker marker;
marker.type = shape; marker.type = shape;
marker.scale.x = 0.1; marker.scale.x = 0.07;
marker.scale.y = 0.1; marker.scale.y = 0.07;
marker.scale.z = 0.1; marker.scale.z = 0.07;
marker.color.r = 0.5; marker.color.r = 0.5;
marker.color.g = 0.5; marker.color.g = 0.5;
marker.color.b = 0.5; marker.color.b = 0.5;
@ -23,7 +24,7 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe
addButtoncontrol(); addButtoncontrol();
} }
//Name must be unique //Construit un InteractiveMarker
void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0)) void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0))
{ {
//// Création d'un marker interactif //// //// Création d'un marker interactif ////
@ -51,6 +52,7 @@ void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector
_server->applyChanges(); _server->applyChanges();
} }
//Fonction callback du serveur d' InteractiveMarker
void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback ) void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback )
{ {
_followObject = false; //Objet manipulé -> on les libèrent _followObject = false; //Objet manipulé -> on les libèrent
@ -94,33 +96,7 @@ void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr
updateVisuals(); updateVisuals();
} }
void InteractiveObject::addVisuals() //Ajoute une fonction bouton
{
// Set the frame ID and timestamp. See the TF tutorials for information on these.
_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 = _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;
//Publie le marker si possible
if( ros::ok() && _visual_pub )
{
_visual_pub->publish(_error_marker);
}
}
void InteractiveObject::addButtoncontrol() void InteractiveObject::addButtoncontrol()
{ {
//// Ajout d'interactions //// //// Ajout d'interactions ////
@ -137,7 +113,7 @@ void InteractiveObject::addButtoncontrol()
_server->applyChanges(); _server->applyChanges();
} }
//6DOF + Boutton //Ajoute des controle pour 6DOF
void InteractiveObject::add6DOFcontrol() void InteractiveObject::add6DOFcontrol()
{ {
//// Ajout d'interactions //// //// Ajout d'interactions ////
@ -193,7 +169,7 @@ void InteractiveObject::add6DOFcontrol()
_server->applyChanges(); _server->applyChanges();
} }
//3DOF + Boutton //Ajoute des controle pour 3DOF
void InteractiveObject::add3DOFcontrol() void InteractiveObject::add3DOFcontrol()
{ {
//// Ajout d'interactions //// //// Ajout d'interactions ////
@ -240,6 +216,7 @@ void InteractiveObject::add3DOFcontrol()
_server->applyChanges(); _server->applyChanges();
} }
//Met à jour la zone d'erreur
void InteractiveObject::setErrorArea(double error) void InteractiveObject::setErrorArea(double error)
{ {
_showVisuals = true; _showVisuals = true;
@ -249,6 +226,7 @@ void InteractiveObject::setErrorArea(double error)
updateVisuals(); updateVisuals();
} }
//Déplace le marker à une position donnée
void InteractiveObject::moveTo(const tf::Vector3& new_pos) void InteractiveObject::moveTo(const tf::Vector3& new_pos)
{ {
tf::pointTFToMsg(new_pos, _int_marker.pose.position); tf::pointTFToMsg(new_pos, _int_marker.pose.position);
@ -258,6 +236,35 @@ void InteractiveObject::moveTo(const tf::Vector3& new_pos)
updateVisuals(); updateVisuals();
} }
//Ajoute des infos visuelles (Zone d'erreur)
void InteractiveObject::addVisuals()
{
// Set the frame ID and timestamp. See the TF tutorials for information on these.
_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 = _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;
//Publie le marker si possible
if( ros::ok() && _visual_pub )
{
_visual_pub->publish(_error_marker);
}
}
//Rafraichis les infos visuelles
void InteractiveObject::updateVisuals() void InteractiveObject::updateVisuals()
{ {
// ROS_INFO_STREAM("Update visuals"); // ROS_INFO_STREAM("Update visuals");

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

@ -13,41 +13,52 @@ class InteractiveObject
protected: protected:
unsigned int _objectID; //Identifiant de l'objet unsigned int _objectID; //Identifiant de l'objet
static unsigned int nextObjectID; static unsigned int nextObjectID;
std::string _name; std::string _name; //Nom de l'objet
unsigned int _type; unsigned int _type; //Type de l'objet
bool _showVisuals; bool _showVisuals; //Indique si les infos visuelles sont affichées
bool _followObject; bool _followObject; //Indique si le marker suit l'objet
//std::vector<visualization_msgs::InteractiveMarker> _int_markers;
InteractiveMarker _int_marker;
// std::vector<Marker> _visual_markers;
Marker _error_marker;
interactive_markers::InteractiveMarkerServer* _server; InteractiveMarker _int_marker; //Marker interactif
rviz_interface::StateSpace _state; Marker _error_marker; //Marker visuel de zone d'erreur
interactive_markers::InteractiveMarkerServer* _server; //Serveur d'InteractiveMaker
rviz_interface::StateSpace _state; //Etat "mémoire"
//Publisher ROS
ros::Publisher* _objective_pub; ros::Publisher* _objective_pub;
ros::Publisher* _visual_pub; ros::Publisher* _visual_pub;
public: public:
InteractiveObject(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);
//Construit un InteractiveMarker
void createInteractiveMarker(Marker& marker, const tf::Vector3& position); void createInteractiveMarker(Marker& marker, const tf::Vector3& position);
//Fonction callback du serveur d' InteractiveMarker
void processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback ); void processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback );
//Ajoute une fonction bouton
void addButtoncontrol(); void addButtoncontrol();
//Ajoute des controle pour 6DOF (Déplacement dans l'espace)
void add6DOFcontrol(); void add6DOFcontrol();
//Ajoute des controle pour 3DOF (Déplacement dans le plan)
void add3DOFcontrol(); void add3DOFcontrol();
//Ajoute des infos visuelles (Zone d'erreur)
void addVisuals(); void addVisuals();
//Rafraichis les infos visuelles
void updateVisuals(); void updateVisuals();
void setObjectivePublisher(ros::Publisher* objective_pub){ _objective_pub=objective_pub;} //Met à jour la zone d'erreur
void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visual_pub=visualization_pub;}
void setErrorArea(double error); void setErrorArea(double error);
//Déplace le marker à une position donnée
void moveTo(const tf::Vector3& new_pos); void moveTo(const tf::Vector3& new_pos);
//Accesseurs
void setObjectivePublisher(ros::Publisher* objective_pub){ _objective_pub=objective_pub;}
void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visual_pub=visualization_pub;}
InteractiveMarker& int_marker(){ return _int_marker;} InteractiveMarker& int_marker(){ return _int_marker;}
Marker& err_marker(){ return _error_marker;} Marker& err_marker(){ return _error_marker;}
rviz_interface::StateSpace& state(){ return _state;} rviz_interface::StateSpace& state(){ return _state;}

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

@ -1,5 +1,6 @@
#include "RvizInterface.hpp" #include "RvizInterface.hpp"
//Constructeur
RvizInterface::RvizInterface(): _server("RvizInterface") RvizInterface::RvizInterface(): _server("RvizInterface")
{ {
//Topic you want to publish //Topic you want to publish
@ -24,6 +25,7 @@ RvizInterface::RvizInterface(): _server("RvizInterface")
} }
} }
//Destructeur
RvizInterface::~RvizInterface() RvizInterface::~RvizInterface()
{ {
for(unsigned int i=0;i<_objects.size();i++) for(unsigned int i=0;i<_objects.size();i++)
@ -32,6 +34,7 @@ RvizInterface::~RvizInterface()
} }
} }
//Fonction Callback du panel Rviz gérant les configurations
void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_config) void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_config)
{ {
for(unsigned int i=0;i<_objects.size();i++) for(unsigned int i=0;i<_objects.size();i++)
@ -47,6 +50,7 @@ void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_c
} }
} }
//Fonction callback gérant la position de l'objet
//PROVISOIRE : Pour un seul objet avec test PCL //PROVISOIRE : Pour un seul objet avec test PCL
void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center) void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center)
{ {

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

@ -14,20 +14,30 @@ class RvizInterface
{ {
protected: protected:
ros::NodeHandle _n; ros::NodeHandle _n;
//Publisher ROS
ros::Publisher _objective_pub; ros::Publisher _objective_pub;
ros::Publisher _visualization_pub; ros::Publisher _visualization_pub;
//Subscriber ROS
ros::Subscriber _config_sub; ros::Subscriber _config_sub;
ros::Subscriber _position_sub; ros::Subscriber _position_sub;
//Serveur de marker interactif
interactive_markers::InteractiveMarkerServer _server; interactive_markers::InteractiveMarkerServer _server;
//Objets de l'interface
std::vector<InteractiveObject*> _objects; std::vector<InteractiveObject*> _objects;
public: public:
RvizInterface(); RvizInterface();
~RvizInterface(); ~RvizInterface();
//Fonction Callback du panel Rviz gérant les configurations
void configCallback(const rviz_interface::InterfaceConfig & new_config); void configCallback(const rviz_interface::InterfaceConfig & new_config);
//Fonction callback gérant la position de l'objet
//PROVISOIRE : Pour un seul objet avec test PCL
void positionCallback(const geometry_msgs::PointStamped & new_center); void positionCallback(const geometry_msgs::PointStamped & new_center);
}; };