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

Association marker/objet

Browse source 
Ajout du Message NamedPoint
Synchronisation des frame_id à approfondir
This commit is contained in:
Unknown 2018-07-02 18:13:46 +02:00
parent 166a69125b
commit edc072b0be
10 changed files with 84 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

9
my_pcl_tutorial/src/plannar_segmentation.cpp
View file

@ -17,6 +17,8 @@
#include <pcl/pcl_config.h>
#include <rviz_interface/NamedPoint.h>
// void printinfo()
// {
// std::cout<<"OK"<<std::endl;
@ -88,7 +90,9 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
Eigen::Vector4f barycentre;
pcl::compute3DCentroid(*pcl_output, barycentre);
geometry_msgs::PointStamped center_msg;
// geometry_msgs::PointStamped center_msg;
rviz_interface::NamedPoint center_msg;
center_msg.name = "6DOF";
// for (size_t i = 0; i < pcl_output->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<sensor_msgs::PointCloud2> ("/plane_output", 1);
pubc = nh.advertise<geometry_msgs::PointStamped> ("/object_center", 1);
// pubc = nh.advertise<geometry_msgs::PointStamped> ("/object_center", 1);
pubc = nh.advertise<rviz_interface::NamedPoint> ("/object_center", 1);
// printinfo();
// Spin

2
rviz_interface/rviz_interface/CMakeLists.txt
View file

@ -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
)
###################################

2
rviz_interface/rviz_interface/CMakeLists.txt~
View file

@ -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
)
###################################

11
rviz_interface/rviz_interface/msg/NamedPoint.msg Normal file
View file

@ -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

11
rviz_interface/rviz_interface/msg/NamedPoint.msg~ Normal file
View file

@ -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

2
rviz_interface/rviz_interface/msg/StateSpace.msg
View file

@ -1,7 +1,7 @@
# Represents a state space.
# General info.
#Header header
Header header
# Identifying string for this state space. (ID ?)
string name

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

@ -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);

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

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

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

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

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

@ -10,6 +10,8 @@
#include <rviz_interface/InterfaceConfig.h>
#include <rviz_interface/NamedPoint.h>
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