From edc072b0be1d60fe94aedb345ffc3240cf8756b4 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 2 Jul 2018 18:13:46 +0200 Subject: [PATCH] Association marker/objet MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Ajout du Message NamedPoint Synchronisation des frame_id à approfondir --- my_pcl_tutorial/src/plannar_segmentation.cpp | 9 ++- rviz_interface/rviz_interface/CMakeLists.txt | 2 + rviz_interface/rviz_interface/CMakeLists.txt~ | 2 + .../rviz_interface/msg/NamedPoint.msg | 11 ++++ .../rviz_interface/msg/NamedPoint.msg~ | 11 ++++ .../rviz_interface/msg/StateSpace.msg | 2 +- .../rviz_interface/src/InteractiveObject.cpp | 5 +- .../rviz_interface/src/InteractiveObject.hpp | 1 + .../rviz_interface/src/RvizInterface.cpp | 56 ++++++++++++++----- .../rviz_interface/src/RvizInterface.hpp | 4 +- 10 files changed, 84 insertions(+), 19 deletions(-) create mode 100644 rviz_interface/rviz_interface/msg/NamedPoint.msg create mode 100644 rviz_interface/rviz_interface/msg/NamedPoint.msg~ diff --git a/my_pcl_tutorial/src/plannar_segmentation.cpp b/my_pcl_tutorial/src/plannar_segmentation.cpp index c56ff5f..c0291ef 100644 --- a/my_pcl_tutorial/src/plannar_segmentation.cpp +++ b/my_pcl_tutorial/src/plannar_segmentation.cpp @@ -17,6 +17,8 @@ #include +#include + // void printinfo() // { // std::cout<<"OK"<size (); ++i) // { // if( pcl_output->points[i].x) @@ -129,7 +133,8 @@ int main (int argc, char** argv) // Create a ROS publisher for the output point cloud pub = nh.advertise ("/plane_output", 1); - pubc = nh.advertise ("/object_center", 1); + // pubc = nh.advertise ("/object_center", 1); + pubc = nh.advertise ("/object_center", 1); // printinfo(); // Spin diff --git a/rviz_interface/rviz_interface/CMakeLists.txt b/rviz_interface/rviz_interface/CMakeLists.txt index f4a4963..7de1a9e 100644 --- a/rviz_interface/rviz_interface/CMakeLists.txt +++ b/rviz_interface/rviz_interface/CMakeLists.txt @@ -38,6 +38,7 @@ add_message_files( FILES StateSpace.msg InterfaceConfig.msg + NamedPoint.msg ) ## Generate services in the 'srv' folder @@ -58,6 +59,7 @@ add_message_files( generate_messages( DEPENDENCIES std_msgs + geometry_msgs ) ################################### diff --git a/rviz_interface/rviz_interface/CMakeLists.txt~ b/rviz_interface/rviz_interface/CMakeLists.txt~ index f4a4963..7de1a9e 100644 --- a/rviz_interface/rviz_interface/CMakeLists.txt~ +++ b/rviz_interface/rviz_interface/CMakeLists.txt~ @@ -38,6 +38,7 @@ add_message_files( FILES StateSpace.msg InterfaceConfig.msg + NamedPoint.msg ) ## Generate services in the 'srv' folder @@ -58,6 +59,7 @@ add_message_files( generate_messages( DEPENDENCIES std_msgs + geometry_msgs ) ################################### diff --git a/rviz_interface/rviz_interface/msg/NamedPoint.msg b/rviz_interface/rviz_interface/msg/NamedPoint.msg new file mode 100644 index 0000000..fa727e0 --- /dev/null +++ b/rviz_interface/rviz_interface/msg/NamedPoint.msg @@ -0,0 +1,11 @@ +#Represents a named point. + +# General info. +Header header + +# Name. +string name + +# 3D point. +geometry_msgs/Point point +#geometry_msgs/Pose pose diff --git a/rviz_interface/rviz_interface/msg/NamedPoint.msg~ b/rviz_interface/rviz_interface/msg/NamedPoint.msg~ new file mode 100644 index 0000000..7625c4b --- /dev/null +++ b/rviz_interface/rviz_interface/msg/NamedPoint.msg~ @@ -0,0 +1,11 @@ +#Represents a named point. + +# General info. +Header header + +# Name. +string name + +# 3D point. +#geometry_msgs/Point point +geometry_msgs/Pose pose diff --git a/rviz_interface/rviz_interface/msg/StateSpace.msg b/rviz_interface/rviz_interface/msg/StateSpace.msg index cb8dca5..6e2a2f6 100644 --- a/rviz_interface/rviz_interface/msg/StateSpace.msg +++ b/rviz_interface/rviz_interface/msg/StateSpace.msg @@ -1,7 +1,7 @@ # Represents a state space. # General info. -#Header header +Header header # Identifying string for this state space. (ID ?) string name diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp index 3b6b020..f846245 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.cpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.cpp @@ -28,8 +28,9 @@ 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"; //Par défaut - _int_marker.header.stamp=ros::Time::now(); + // _int_marker.header.frame_id = "/map"; //Par défaut + _int_marker.header.frame_id = "/camera_rgb_optical_frame"; + _int_marker.header.stamp= ros::Time::now(); _int_marker.name = _name; //ATTENTION ! _int_marker.description = _name; tf::pointTFToMsg(position, _int_marker.pose.position); diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.hpp b/rviz_interface/rviz_interface/src/InteractiveObject.hpp index 33f3cfa..fa23966 100644 --- a/rviz_interface/rviz_interface/src/InteractiveObject.hpp +++ b/rviz_interface/rviz_interface/src/InteractiveObject.hpp @@ -59,6 +59,7 @@ public: void moveTo(const tf::Vector3& new_pos); //Accesseurs + const std::string& name() const{ return _name;} 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;} diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp index 2d98292..cbc8cf5 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.cpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -11,18 +11,27 @@ RvizInterface::RvizInterface(): _server("RvizInterface") _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.push_back(new InteractiveObject(&_server, "6DOF", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0))); _objects[0]->add6DOFcontrol(); - // _objects[0]->add3DOFcontrol(tf::Vector3(0.5,0.5,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[1]->add3DOFcontrol(); + _objects.push_back(new InteractiveObject(&_server, "3DOF", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,1,0))); + _objects[1]->add3DOFcontrol(); + + + // _objects.push_back(new InteractiveObject(&_server, "3DOF 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(-1,1,0))); + // _objects[2]->add3DOFcontrol(tf::Vector3(0.5,0.5,0)); + + // _objects.push_back(new InteractiveObject(&_server, "Button", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,-1,0))); for(unsigned int i=0;i<_objects.size();i++) { _objects[i]->setObjectivePublisher(&_objective_pub); _objects[i]->setVisualizationPublisher(&_visualization_pub); _objects[i]->addVisuals(); + + //Synchro des frames + // _objects[i]->int_marker().header.frame_id = "/camera_rgb_optical_frame"; + // _objects[i]->err_marker().header.frame_id = "/camera_rgb_optical_frame"; } } @@ -53,17 +62,38 @@ 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) -{ - //Synchro des frames - _objects[0]->int_marker().header.frame_id = new_center.header.frame_id; - _objects[0]->err_marker().header.frame_id = new_center.header.frame_id; +// void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center) +// { +// //Synchro des frames +// _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 +// if(_objects[0]->isFollowing()) //Le marqueur suit l'objet +// { +// //Deplacement du marqueur +// tf::Vector3 position = tf::Vector3(new_center.point.x,new_center.point.y,new_center.point.z); +// _objects[0]->moveTo(position); +// } +// } + +void RvizInterface::positionCallback(const rviz_interface::NamedPoint & new_center) +{ + for(unsigned int i=0;i<_objects.size();i++) { - //Deplacement du marqueur - tf::Vector3 position = tf::Vector3(new_center.point.x,new_center.point.y,new_center.point.z); - _objects[0]->moveTo(position); + if(_objects[i]->name()==new_center.name) + { + //Synchro des frames + // _objects[i]->int_marker().header.frame_id = new_center.header.frame_id; + // _objects[i]->err_marker().header.frame_id = new_center.header.frame_id; + + if(_objects[i]->isFollowing()) //Le marqueur suit l'objet + { + //Deplacement du marqueur + tf::Vector3 position = tf::Vector3(new_center.point.x,new_center.point.y,new_center.point.z); + _objects[i]->moveTo(position); + } + break; + } } } diff --git a/rviz_interface/rviz_interface/src/RvizInterface.hpp b/rviz_interface/rviz_interface/src/RvizInterface.hpp index 66dee35..73741b6 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.hpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.hpp @@ -10,6 +10,8 @@ #include +#include + class RvizInterface { protected: @@ -38,7 +40,7 @@ public: //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 rviz_interface::NamedPoint & new_center); }; #endif \ No newline at end of file