diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp index 425b48f..8cb777d 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.cpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.cpp @@ -2,6 +2,7 @@ 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) { _objectID = nextObjectID++; @@ -11,9 +12,9 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe Marker marker; marker.type = shape; - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.scale.z = 0.1; + marker.scale.x = 0.07; + marker.scale.y = 0.07; + marker.scale.z = 0.07; marker.color.r = 0.5; marker.color.g = 0.5; marker.color.b = 0.5; @@ -23,7 +24,7 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe addButtoncontrol(); } - //Name must be unique + //Construit un InteractiveMarker void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0)) { //// Création d'un marker interactif //// @@ -51,6 +52,7 @@ void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector _server->applyChanges(); } +//Fonction callback du serveur d' InteractiveMarker void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback ) { _followObject = false; //Objet manipulé -> on les libèrent @@ -94,33 +96,7 @@ void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr updateVisuals(); } -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); - } -} - +//Ajoute une fonction bouton void InteractiveObject::addButtoncontrol() { //// Ajout d'interactions //// @@ -137,7 +113,7 @@ void InteractiveObject::addButtoncontrol() _server->applyChanges(); } -//6DOF + Boutton +//Ajoute des controle pour 6DOF void InteractiveObject::add6DOFcontrol() { //// Ajout d'interactions //// @@ -193,7 +169,7 @@ void InteractiveObject::add6DOFcontrol() _server->applyChanges(); } -//3DOF + Boutton +//Ajoute des controle pour 3DOF void InteractiveObject::add3DOFcontrol() { //// Ajout d'interactions //// @@ -240,6 +216,7 @@ void InteractiveObject::add3DOFcontrol() _server->applyChanges(); } +//Met à jour la zone d'erreur void InteractiveObject::setErrorArea(double error) { _showVisuals = true; @@ -249,6 +226,7 @@ void InteractiveObject::setErrorArea(double error) updateVisuals(); } +//Déplace le marker à une position donnée void InteractiveObject::moveTo(const tf::Vector3& new_pos) { tf::pointTFToMsg(new_pos, _int_marker.pose.position); @@ -258,6 +236,35 @@ void InteractiveObject::moveTo(const tf::Vector3& new_pos) 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() { // ROS_INFO_STREAM("Update visuals"); diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.hpp b/rviz_interface/rviz_interface/src/InteractiveObject.hpp index 9ebd7c9..d351967 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.hpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.hpp @@ -13,41 +13,52 @@ class InteractiveObject protected: unsigned int _objectID; //Identifiant de l'objet static unsigned int nextObjectID; - std::string _name; - unsigned int _type; - bool _showVisuals; - bool _followObject; - //std::vector _int_markers; - InteractiveMarker _int_marker; - // std::vector _visual_markers; - Marker _error_marker; + std::string _name; //Nom de l'objet + unsigned int _type; //Type de l'objet + bool _showVisuals; //Indique si les infos visuelles sont affichées + bool _followObject; //Indique si le marker suit l'objet - interactive_markers::InteractiveMarkerServer* _server; - rviz_interface::StateSpace _state; + InteractiveMarker _int_marker; //Marker interactif + 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* _visual_pub; public: 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); + //Fonction callback du serveur d' InteractiveMarker void processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback ); - + + //Ajoute une fonction bouton void addButtoncontrol(); + //Ajoute des controle pour 6DOF (Déplacement dans l'espace) void add6DOFcontrol(); + //Ajoute des controle pour 3DOF (Déplacement dans le plan) void add3DOFcontrol(); + //Ajoute des infos visuelles (Zone d'erreur) void addVisuals(); + //Rafraichis les infos visuelles void updateVisuals(); - void setObjectivePublisher(ros::Publisher* objective_pub){ _objective_pub=objective_pub;} - void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visual_pub=visualization_pub;} + //Met à jour la zone d'erreur void setErrorArea(double error); + //Déplace le marker à une position donnée 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;} Marker& err_marker(){ return _error_marker;} rviz_interface::StateSpace& state(){ return _state;} diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp index 401d31b..4104da8 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.cpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -1,5 +1,6 @@ #include "RvizInterface.hpp" +//Constructeur RvizInterface::RvizInterface(): _server("RvizInterface") { //Topic you want to publish @@ -24,6 +25,7 @@ RvizInterface::RvizInterface(): _server("RvizInterface") } } +//Destructeur RvizInterface::~RvizInterface() { 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) { 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 void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center) { diff --git a/rviz_interface/rviz_interface/src/RvizInterface.hpp b/rviz_interface/rviz_interface/src/RvizInterface.hpp index 0ee7f7c..66dee35 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.hpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.hpp @@ -14,20 +14,30 @@ class RvizInterface { protected: ros::NodeHandle _n; + + //Publisher ROS ros::Publisher _objective_pub; ros::Publisher _visualization_pub; + + //Subscriber ROS ros::Subscriber _config_sub; ros::Subscriber _position_sub; + //Serveur de marker interactif interactive_markers::InteractiveMarkerServer _server; + //Objets de l'interface std::vector _objects; public: RvizInterface(); ~RvizInterface(); + //Fonction Callback du panel Rviz gérant les configurations 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); };