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

Ajout du suivie du marker

Browse source
This commit is contained in:
Unknown 2018-06-29 15:31:04 +02:00
parent 656305cd8a
commit 987720b171
8 changed files with 65 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

2
my_pcl_tutorial/src/plannar_segmentation.cpp
View file

@ -106,7 +106,7 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
center_msg.header.frame_id = input->header.frame_id;
// ROS_INFO_STREAM("Center clioyf : "<<pcl_output->points[0]._PointXYZ::data[0]<<" / "<<pcl_output->points[0]._PointXYZ::data[1]<<" / "<<pcl_output->points[0]._PointXYZ::data[2]);
// ROS_INFO_STREAM("barycentre : "<<barycentre(1,1)<<" / "<<barycentre(2,1)<<" / "<<barycentre(3,1)<<" / "<< barycentre(4,1));
ROS_INFO_STREAM("Center : "<<center_msg.point.x<<" / "<<center_msg.point.y<<" / "<<center_msg.point.z);
// ROS_INFO_STREAM("Center : "<<center_msg.point.x<<" / "<<center_msg.point.y<<" / "<<center_msg.point.z);
//Envoie du nuage de points
pcl::toPCLPointCloud2(*pcl_output,pcl_pc2);

3
rviz_interface/rviz_interface/msg/InterfaceConfig.msg
View file

@ -9,3 +9,6 @@ uint8 objective_type
#Maximum distance error allowed arround the objective
float32 max_error
#Follow Object: boolean (true : les marqueur suivent les objets)
uint8 follow_object

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

@ -2,7 +2,7 @@
unsigned int InteractiveObject::nextObjectID = 1;
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)
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++;
_state.name = name;
@ -11,9 +11,9 @@ InteractiveObject::InteractiveObject(interactive_markers::InteractiveMarkerServe
Marker marker;
marker.type = shape;
marker.scale.x = 0.45;
marker.scale.y = 0.45;
marker.scale.z = 0.45;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
@ -52,6 +52,8 @@ void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector
void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback )
{
_followObject = false; //Objet manipulé -> on les libèrent
//// Update Visual markers ////
// if(_showVisuals)
// {
@ -277,4 +279,11 @@ void InteractiveObject::setErrorArea(double error)
_showVisuals = true;
_state.max_error = error;
}
void InteractiveObject::moveTo(const tf::Vector3& new_pos)
{
tf::pointTFToMsg(new_pos, _int_marker.pose.position);
_server->insert(_int_marker);
_server->applyChanges();
}

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

@ -16,6 +16,7 @@ protected:
std::string _name;
unsigned int _type;
bool _showVisuals;
bool _followObject;
//std::vector<visualization_msgs::InteractiveMarker> _int_markers;
InteractiveMarker _int_marker;
// std::vector<Marker> _visual_markers;
@ -43,9 +44,13 @@ public:
// void setVisualizationPublisher(ros::Publisher* visualization_pub){ _visualization_pub=visualization_pub;}
void setErrorArea(double error);
void moveTo(const tf::Vector3& new_pos);
//std::vector<visualization_msgs::InteractiveMarker>& markers(){ return _int_markers;}
InteractiveMarker& marker(){ return _int_marker;}
rviz_interface::StateSpace& state(){ return _state;}
bool isFollowing() const{ return _followObject;}
void follow(){ _followObject = true;}
};
#endif

14
rviz_interface/rviz_interface/src/InterfacePanel.cpp
View file

@ -32,6 +32,9 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent )
// Lay out the topic field above the control widget.
QVBoxLayout* layout = new QVBoxLayout;
_objective_type_editor = new QCheckBox( "Precise Objective" );
_reset_button = new QPushButton("Reset", this);
layout->addWidget( _reset_button );
layout->addWidget( _objective_type_editor );
layout->addLayout( error_layout );
setLayout( layout );
@ -50,6 +53,7 @@ InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent )
// Next we make signal/slot connections.
connect( _max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() ));
connect( _objective_type_editor, SIGNAL( stateChanged(int) ), this, SLOT( updateType(int) ));
connect(_reset_button, SIGNAL (released()), this, SLOT (handleResetButton()));
// connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));
// Start the timer.
@ -92,6 +96,16 @@ void InterfacePanel::updateType(int state)
_max_error_editor->setEnabled( state ); //Active l'editeur d'erreur si state>0 (ie Objectif précis)
}
void InterfacePanel::handleResetButton()
{
current_config.follow_object = true;
if( ros::ok() && _config_publisher )
{
_config_publisher.publish( current_config );
}
current_config.follow_object = false;
}
// void InterfacePanel::setError( const QString& error )
// {
// current_config.max_error = error.toDouble();

3
rviz_interface/rviz_interface/src/InterfacePanel.hpp
View file

@ -13,6 +13,7 @@
#include <QLineEdit>
// #include <QDoubleValidator>
#include <QCheckBox>
#include <QPushButton>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
@ -64,6 +65,7 @@ protected Q_SLOTS:
void updateError();
void updateType(int state);
void handleResetButton();
// Then we finish up with protected member variables.
@ -74,6 +76,7 @@ protected:
// One-line text editor for entering the outgoing ROS topic name.
QLineEdit* _max_error_editor;
QCheckBox* _objective_type_editor;
QPushButton* _reset_button;
// The current name of the output topic.
// QString _max_error;

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

@ -8,14 +8,15 @@ RvizInterface::RvizInterface(): _server("RvizInterface")
//Topic you want to subscribe
_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::CUBE, tf::Vector3(0,0,0)));
_objects.push_back(new InteractiveObject(&_server, "Test", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
// _objects[0].addButtonControl();
_objects[0]->add6DOFcontrol();
_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();
// _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++)
{
@ -40,6 +41,25 @@ void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_c
_objects[i]->state().objective_type = new_config.objective_type;
// _objects[i]->state().max_error = new_config.max_error;
_objects[i]->setErrorArea(new_config.max_error);
if(new_config.follow_object && !_objects[i]->isFollowing()) //Signal reset + pas de suivie
{
_objects[i]->follow(); //Le ùarquer suit de nouveau l'objet
}
}
}
//PROVISOIRE : Pour un seul objet avec test PCL
void RvizInterface::positionCallback(const geometry_msgs::PointStamped & new_center)
{
//Synchro des frames
_objects[0]->marker().header.frame_id = new_center.header.frame_id;
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);
}
}

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

@ -17,6 +17,7 @@ protected:
ros::Publisher _objective_pub;
ros::Publisher _visualization_pub;
ros::Subscriber _config_sub;
ros::Subscriber _position_sub;
interactive_markers::InteractiveMarkerServer _server;
@ -27,6 +28,7 @@ public:
~RvizInterface();
void configCallback(const rviz_interface::InterfaceConfig & new_config);
void positionCallback(const geometry_msgs::PointStamped & new_center);
};
#endif