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

Initial commit

Browse source
This commit is contained in:
Unknown 2018-06-26 18:27:30 +02:00
parent 8479b32a78
commit 022fcb2c1c
32 changed files with 3116 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

432
rviz_interface/interface_tests/src/interface_test.cpp Normal file
View file

@ -0,0 +1,432 @@
#include <ros/ros.h>
#include <interactive_markers/interactive_marker_server.h>
#include <tf/tf.h>
#include <interface_tests/StateSpaceT.h>
using namespace visualization_msgs;
// class InteractiveObject
// {
// protected:
// unsigned int _objectID; //Identifiant de l'objet
// static unsigned int nextObjectID;
// std::string _name;
// unsigned int _type;
// //std::vector<visualization_msgs::InteractiveMarker> _int_markers;
// visualization_msgs::InteractiveMarker _int_marker;
// interactive_markers::InteractiveMarkerServer& _server;
// interface_tests::StateSpaceT _state;
// public:
// InteractiveObject(const std::string& name, unsigned int type, interactive_markers::InteractiveMarkerServer& server): _name(name), _type(type), _server(server)
// {
// _objectID = nextObjectID++;
// _state.state_type=type;
// //_server = server;
// //_int_marker = createInteractiveMarker(_name+_objectID,box_marker);
// }
// //std::vector<visualization_msgs::InteractiveMarker>& markers(){ return _int_markers;}
// visualization_msgs::InteractiveMarker& marker(){ return _int_marker;}
// interface_tests::StateSpaceT& state(){ return _state;}
// };
// unsigned int InteractiveObject::nextObjectID = 1;
class InterfaceTest
{
private :
ros::NodeHandle _n;
ros::Publisher _objective_pub;
//ros::Subscriber sub_;
interactive_markers::InteractiveMarkerServer server;
//std::vector<visualization_msgs::InteractiveMarker> _int_markers;
// std::vector<InteractiveObject> _objects;
public:
InterfaceTest(): server("interface_tests")
{
//Topic you want to publish
_objective_pub = _n.advertise<interface_tests::StateSpaceT>("state_objective", 10);
//Topic you want to subscribe
//sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this);
// create a grey box marker
visualization_msgs::Marker box_marker;
box_marker.type = visualization_msgs::Marker::CUBE;
box_marker.scale.x = 0.45;
box_marker.scale.y = 0.45;
box_marker.scale.z = 0.45;
box_marker.color.r = 0.5;
box_marker.color.g = 0.5;
box_marker.color.b = 0.5;
box_marker.color.a = 1.0;
tf::Vector3 position;
position = tf::Vector3(0, 0, 0);
visualization_msgs::InteractiveMarker int_box = createInteractiveMarker("6DOF 1",box_marker);
addButtoncontrol(int_box);
add6DOFcontrol(int_box);
visualization_msgs::Marker sphere_marker;
sphere_marker.type = visualization_msgs::Marker::SPHERE;
sphere_marker.scale.x = 0.45;
sphere_marker.scale.y = 0.45;
sphere_marker.scale.z = 0.45;
sphere_marker.color.r = 0.5;
sphere_marker.color.g = 0.5;
sphere_marker.color.b = 0.5;
sphere_marker.color.a = 1.0;
position = tf::Vector3(-3, 3, 0);
visualization_msgs::InteractiveMarker int_sphere = createInteractiveMarker("3DOF",sphere_marker,position);
addButtoncontrol(int_sphere);
add3DOFcontrol(int_sphere);
visualization_msgs::Marker cyl_marker;
cyl_marker.type = visualization_msgs::Marker::CYLINDER;
cyl_marker.scale.x = 0.45;
cyl_marker.scale.y = 0.45;
cyl_marker.scale.z = 0.45;
cyl_marker.color.r = 0.5;
cyl_marker.color.g = 0.5;
cyl_marker.color.b = 0.5;
cyl_marker.color.a = 1.0;
position = tf::Vector3(3, 3, 0);
visualization_msgs::InteractiveMarker int_cyl = createInteractiveMarker("Button",cyl_marker, position);
addButtoncontrol(int_cyl);
}
//Name must be unique
visualization_msgs::InteractiveMarker createInteractiveMarker(const std::string& name, visualization_msgs::Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0))
{
//// Création d'un marker interactif ////
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = "base_link";
int_marker.header.stamp=ros::Time::now();
int_marker.name = name;
int_marker.description = name;
tf::pointTFToMsg(position, int_marker.pose.position);
// create a non-interactive control which contains the box
visualization_msgs::InteractiveMarkerControl container;
container.always_visible = true; //Toujours visible hors du mode interaction
container.markers.push_back( marker );
// add the control to the interactive marker
int_marker.controls.push_back( container );
// _int_markers.push_back(int_marker);
// add the interactive marker to our collection &
// tell the server to call processFeedback() when feedback arrives for it
//server.insert(_int_marker, &InterfaceTest::processFeedback);
server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
// 'commit' changes and send to all clients
server.applyChanges();
return int_marker;
}
void addButtoncontrol(visualization_msgs::InteractiveMarker& int_marker)
{
//// Ajout d'interactions ////
visualization_msgs::InteractiveMarkerControl marker_control;
int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON;
int_marker.controls[0].name= "Button";
// add the interactive marker to our collection &
// tell the server to call processFeedback() when feedback arrives for it
//server.insert(_int_marker, &InterfaceTest::processFeedback);
server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
// 'commit' changes and send to all clients
server.applyChanges();
}
//6DOF + Boutton
//Name must be unique
void add6DOFcontrol(visualization_msgs::InteractiveMarker& int_marker)
{
//// Ajout d'interactions ////
// create a control which will move the box
// this control does not contain any markers,
// which will cause RViz to insert two arrows
visualization_msgs::InteractiveMarkerControl marker_control;
//_int_marker.controls[0].interaction_mode = InteractiveMarkerControl::MOVE_ROTATE_3D; //Deplacement par "grab" de la souris (CTRL + Souris = rotation)
//int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; //Ajout fonctionalité Boutton (Validation ?)
int_marker.controls[0].name= "3D";
marker_control.orientation_mode = InteractiveMarkerControl::FIXED;
//Orientation du vecteur
marker_control.orientation.w = 1;
marker_control.orientation.x = 1;
marker_control.orientation.y = 0;
marker_control.orientation.z = 0;
//Ajout des controles associées
marker_control.name = "rotate_x";
marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(marker_control);
marker_control.name = "move_x";
marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(marker_control);
//Orientation du vecteur
marker_control.orientation.w = 1;
marker_control.orientation.x = 0;
marker_control.orientation.y = 1;
marker_control.orientation.z = 0;
//Ajout des controles associées
marker_control.name = "rotate_y";
marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(marker_control);
marker_control.name = "move_y";
marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(marker_control);
//Orientation du vecteur
marker_control.orientation.w = 1;
marker_control.orientation.x = 0;
marker_control.orientation.y = 0;
marker_control.orientation.z = 1;
//Ajout des controles associées
marker_control.name = "rotate_z";
marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(marker_control);
marker_control.name = "move_z";
marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(marker_control);
// _int_markers.push_back(int_marker);
// add the interactive marker to our collection &
// tell the server to call processFeedback() when feedback arrives for it
//server.insert(_int_marker, &InterfaceTest::processFeedback);
server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
// 'commit' changes and send to all clients
server.applyChanges();
}
//3DOF + Boutton
//Name must be unique
void add3DOFcontrol(visualization_msgs::InteractiveMarker& int_marker)
{
//// Ajout d'interactions ////
// create a control which will move the box
// this control does not contain any markers,
// which will cause RViz to insert two arrows
visualization_msgs::InteractiveMarkerControl marker_control;
//_int_marker.controls[0].interaction_mode = InteractiveMarkerControl::MOVE_ROTATE_3D; //Deplacement par "grab" de la souris (CTRL + Souris = rotation)
// int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; //Ajout fonctionalité Boutton (Validation ?)
int_marker.controls[0].name= "2D";
marker_control.orientation_mode = InteractiveMarkerControl::FIXED;
//Orientation du vecteur
marker_control.orientation.w = 1;
marker_control.orientation.x = 1;
marker_control.orientation.y = 0;
marker_control.orientation.z = 0;
//Ajout des controles associées
marker_control.name = "move_x";
marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(marker_control);
//Orientation du vecteur
marker_control.orientation.w = 1;
marker_control.orientation.x = 0;
marker_control.orientation.y = 1;
marker_control.orientation.z = 0;
//Ajout des controles associées
marker_control.name = "rotate_y";
marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(marker_control);
//Orientation du vecteur
marker_control.orientation.w = 1;
marker_control.orientation.x = 0;
marker_control.orientation.y = 0;
marker_control.orientation.z = 1;
//Ajout des controles associées
marker_control.name = "move_z";
marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(marker_control);
// _int_markers.push_back(int_marker);
// add the interactive marker to our collection &
// tell the server to call processFeedback() when feedback arrives for it
//server.insert(_int_marker, &InterfaceTest::processFeedback);
server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test
// 'commit' changes and send to all clients
server.applyChanges();
}
//Faire deffedback different pour chaque type ?
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
//// SEND OBJECTIVE ////
if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK)
{
interface_tests::StateSpaceT obj;
obj.name = feedback->marker_name +" objective - "+ feedback->control_name;
if(feedback->control_name == "Button")
{
obj.state_type = 3; //interface_tests::STATE_BOOL;
obj.discrete_data.push_back(true);
}
else if(feedback->control_name == "2D")
{
obj.state_type = 1; //interface_tests::STATE_2D;
obj.real_data.push_back(feedback->pose.position.x);
obj.real_data.push_back(feedback->pose.position.y);
// obj.real_data.push_back(feedback->pose.position.z);
// obj.real_data.push_back(feedback->pose.orientation.w);
// obj.real_data.push_back(feedback->pose.orientation.x);
// obj.real_data.push_back(feedback->pose.orientation.y);
obj.real_data.push_back(feedback->pose.orientation.z);
}
else if(feedback->control_name == "3D")
{
obj.state_type = 2; //interface_tests::STATE_3D;
obj.real_data.push_back(feedback->pose.position.x);
obj.real_data.push_back(feedback->pose.position.y);
obj.real_data.push_back(feedback->pose.position.z);
obj.real_data.push_back(feedback->pose.orientation.w);
obj.real_data.push_back(feedback->pose.orientation.x);
obj.real_data.push_back(feedback->pose.orientation.y);
obj.real_data.push_back(feedback->pose.orientation.z);
}
else
{
obj.state_type = 0; // interface_tests::ORDINARY;
obj.real_data.push_back(feedback->pose.position.x);
obj.real_data.push_back(feedback->pose.position.y);
obj.real_data.push_back(feedback->pose.position.z);
obj.real_data.push_back(feedback->pose.orientation.w);
obj.real_data.push_back(feedback->pose.orientation.x);
obj.real_data.push_back(feedback->pose.orientation.y);
obj.real_data.push_back(feedback->pose.orientation.z);
}
//obj.discrete_data.push_back(1);
//obj.discrete_data.push_back(false);
//obj.dimension = obj.real_data.size() + obj.discrete_data.size();
obj.description="Test du message StateSpaceT !";
_objective_pub.publish(obj);
}
//// PRINT INFO ////
// ROS_INFO_STREAM( feedback->marker_name << " is now at "
// << feedback->pose.position.x << ", " << feedback->pose.position.y
// << ", " << feedback->pose.position.z );
// std::ostringstream s;
// s << "Feedback from marker '" << feedback->marker_name << "' "<< " / control '" << feedback->control_name << "'";
// std::ostringstream mouse_point_ss;
// if( feedback->mouse_point_valid )
// {
// mouse_point_ss << " at " << feedback->mouse_point.x
// << ", " << feedback->mouse_point.y
// << ", " << feedback->mouse_point.z
// << " in frame " << feedback->header.frame_id;
// }
// switch ( feedback->event_type )
// {
// case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
// ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
// break;
// case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
// ROS_INFO_STREAM( s.str() << ": pose changed"
// << "\nposition = "
// << feedback->pose.position.x
// << ", " << feedback->pose.position.y
// << ", " << feedback->pose.position.z
// << "\norientation = "
// << feedback->pose.orientation.w
// << ", " << feedback->pose.orientation.x
// << ", " << feedback->pose.orientation.y
// << ", " << feedback->pose.orientation.z
// << "\nframe: " << feedback->header.frame_id
// << " time: " << feedback->header.stamp.sec << "sec, "
// << feedback->header.stamp.nsec << " nsec" );
// break;
// case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
// ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
// break;
// case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
// ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
// break;
// }
}
// Marker makeBox( InteractiveMarker &msg )
// {
// Marker marker;
// marker.type = Marker::CUBE;
// marker.scale.x = msg.scale * 0.45;
// marker.scale.y = msg.scale * 0.45;
// marker.scale.z = msg.scale * 0.45;
// marker.color.r = 0.5;
// marker.color.g = 0.5;
// marker.color.b = 0.5;
// marker.color.a = 1.0;
// return marker;
// }
// InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
// {
// InteractiveMarkerControl control;
// control.always_visible = true;
// control.markers.push_back( makeBox(msg) );
// msg.controls.push_back( control );
// return msg.controls.back();
// }
//const std::vector<visualization_msgs::InteractiveMarker>& markers() const { return _int_markers;}
// std::vector<visualization_msgs::InteractiveMarker>& markers() { return _int_markers;}
};
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "interface_tests");
InterfaceTest Interface;
ros::spin();
return 0;
}

121
rviz_interface/interface_tests/src/listener_test.cpp Normal file
View file

@ -0,0 +1,121 @@
#include "ros/ros.h"
#include <interface_tests/StateSpaceT.h>
/**
* This tutorial demonstrates simple receipt of messages over the ROS system.
*/
void objectiveCallback(const interface_tests::StateSpaceT& msg)
{
std::ostringstream s;
s << "Current Objective : " << std::endl << " Name : "<< msg.name << std::endl;
//s << " Dimension :" << (int) msg.dimension << std::endl;
std::vector<uint8_t> discrete_data = msg.discrete_data;
std::vector<float> real_data = msg.real_data;
switch(msg.state_type)
{
case(interface_tests::StateSpaceT::STATE_BOOL):
if(discrete_data.size() !=1)
ROS_WARN("Receiving STATE_BOOL objective with invalid dimension");
else
{
s << " Switch State !" << std::endl;
}
break;
case(interface_tests::StateSpaceT::STATE_2D):
if(real_data.size() != 3)
ROS_WARN("Receiving STATE_2D objective with invalid dimension");
else
{
s << " Position : " << std::endl;
s << " x : "<< real_data[0] << std::endl;
s << " y : "<< real_data[1] << std::endl;
s << " Orientation : " << std::endl;
s << " theta : "<< real_data[2] << std::endl;
}
break;
case(interface_tests::StateSpaceT::STATE_3D):
if(real_data.size() != 7)
ROS_WARN("Receiving STATE_3D objective with invalid dimension");
else
{
s << " Position : " << std::endl;
s << " x : "<< real_data[0] << std::endl;
s << " y : "<< real_data[1] << std::endl;
s << " z : "<< real_data[2] << std::endl;
s << " Orientation : " << std::endl;
s << " w : "<< real_data[3] << std::endl;
s << " x : "<< real_data[4] << std::endl;
s << " y : "<< real_data[5] << std::endl;
s << " z : "<< real_data[6] << std::endl;
}
break;
case(interface_tests::StateSpaceT::ORDINARY):
s << " Discrete data : "<< discrete_data.size() << std::endl;
for(unsigned int i =0; i<discrete_data.size();i++)
{
s << " "<< discrete_data[i]<<std::endl;
}
s << " Real data : "<< real_data.size() << std::endl;
for(unsigned int i =0; i<real_data.size();i++)
{
s << " "<< real_data[i]<<std::endl;
}
break;
}
ROS_INFO_STREAM(s.str());
}
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "listener");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The subscribe() call is how you tell ROS that you want to receive messages
* on a given topic. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. Messages are passed to a callback function, here
* called chatterCallback. subscribe() returns a Subscriber object that you
* must hold on to until you want to unsubscribe. When all copies of the Subscriber
* object go out of scope, this callback will automatically be unsubscribed from
* this topic.
*
* The second parameter to the subscribe() function is the size of the message
* queue. If messages are arriving faster than they are being processed, this
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
ros::Subscriber sub = n.subscribe("state_objective", 1000, objectiveCallback);
/**
* ros::spin() will enter a loop, pumping callbacks. With this version, all
* callbacks will be called from within this thread (the main one). ros::spin()
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
*/
ros::spin();
return 0;
}

292
rviz_interface/interface_tests/src/marker_test.cpp Normal file
View file

@ -0,0 +1,292 @@
/*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// %Tag(fullSource)%
#include <ros/ros.h>
#include <interactive_markers/interactive_marker_server.h>
//#include <tf/tf.h>
#include <interface_tests/StateSpaceT.h>
using namespace visualization_msgs;
Marker makeBox( InteractiveMarker &msg )
{
Marker marker;
marker.type = Marker::CUBE;
marker.scale.x = msg.scale * 0.45;
marker.scale.y = msg.scale * 0.45;
marker.scale.z = msg.scale * 0.45;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
marker.color.a = 1.0;
return marker;
}
InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
{
InteractiveMarkerControl control;
control.always_visible = true;
control.markers.push_back( makeBox(msg) );
msg.controls.push_back( control );
return msg.controls.back();
}
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
//// SEND OBJECTIVE ////
if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK)
{
//TRES DEGEU -> FAIRE EN OBJET
//ros::Publisher objective_pub = n.advertise<interface_tests::StateSpaceT>("state_objective", 10);
interface_tests::StateSpaceT obj;
obj.name ="Objective_test";
// obj.state_type = interface_tests::STATE_2D;
obj.state_type = 1;
//obj.dimension = 3;
obj.discrete_data.push_back(1);
obj.discrete_data.push_back(false);
obj.real_data.push_back(10.2);
obj.description="Test du message StateSpaceT !";
//objective_pub.publish(obj);
}
//// PRINT INFO ////
// ROS_INFO_STREAM( feedback->marker_name << " is now at "
// << feedback->pose.position.x << ", " << feedback->pose.position.y
// << ", " << feedback->pose.position.z );
std::ostringstream s;
s << "Feedback from marker '" << feedback->marker_name << "' "<< " / control '" << feedback->control_name << "'";
std::ostringstream mouse_point_ss;
if( feedback->mouse_point_valid )
{
mouse_point_ss << " at " << feedback->mouse_point.x
<< ", " << feedback->mouse_point.y
<< ", " << feedback->mouse_point.z
<< " in frame " << feedback->header.frame_id;
}
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
break;
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
ROS_INFO_STREAM( s.str() << ": pose changed"
<< "\nposition = "
<< feedback->pose.position.x
<< ", " << feedback->pose.position.y
<< ", " << feedback->pose.position.z
<< "\norientation = "
<< feedback->pose.orientation.w
<< ", " << feedback->pose.orientation.x
<< ", " << feedback->pose.orientation.y
<< ", " << feedback->pose.orientation.z
<< "\nframe: " << feedback->header.frame_id
<< " time: " << feedback->header.stamp.sec << "sec, "
<< feedback->header.stamp.nsec << " nsec" );
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
break;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "marker_test");
ros::NodeHandle n;
//Create Objective topic
ros::Publisher objective_pub = n.advertise<interface_tests::StateSpaceT>("state_objective", 10);
//Test du topic
interface_tests::StateSpaceT obj;
obj.name ="Objective_test";
// obj.state_type = interface_tests::STATE_2D;
obj.state_type = 1;
//obj.dimension = 3;
obj.discrete_data.push_back(1);
obj.discrete_data.push_back(false);
obj.real_data.push_back(10.2);
obj.description="Test du message StateSpaceT !";
objective_pub.publish(obj);
ROS_INFO_STREAM("Starting Marker Test");
// create an interactive marker server on the topic namespace simple_marker
interactive_markers::InteractiveMarkerServer server("marker_test");
// create an interactive marker for our server
visualization_msgs::InteractiveMarker int_marker;
int_marker.header.frame_id = "base_link";
int_marker.header.stamp=ros::Time::now();
int_marker.name = "my_marker";
int_marker.description = "Test Control";
// create a grey box marker
visualization_msgs::Marker box_marker;
box_marker.type = visualization_msgs::Marker::CUBE;
box_marker.scale.x = 0.45;
box_marker.scale.y = 0.45;
box_marker.scale.z = 0.45;
box_marker.color.r = 0.5;
box_marker.color.g = 0.5;
box_marker.color.b = 0.5;
box_marker.color.a = 1.0;
// create a non-interactive control which contains the box
visualization_msgs::InteractiveMarkerControl box_control;
box_control.always_visible = true; //Toujours visible hors du mode interaction
box_control.markers.push_back( box_marker );
// add the control to the interactive marker
int_marker.controls.push_back( box_control );
// create a control which will move the box
// this control does not contain any markers,
// which will cause RViz to insert two arrows
visualization_msgs::InteractiveMarkerControl new_control;
new_control.name = "move_test";
//MODE 3D fonctionnel ???
// new_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
// add the control to the interactive marker
//int_marker.controls.push_back(new_control);
//Orientation du vecteur
// new_control.orientation.w = 1;
// new_control.orientation.x = 0;
// new_control.orientation.y = 1;
// new_control.orientation.z = 0;
// new_control.name = "rotate_y";
// new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
// int_marker.controls.push_back(new_control);
// new_control.name = "move_plane_y";
// new_control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; //Attention par défaut utilise un halo (comme le rotate Axis) peut etre ignorer -> utiliser MOVE_ROTATE dans ce cas
// int_marker.controls.push_back(new_control);
// new_control.name = "move_rotate_y";
// new_control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE; //Difficile pour des mouvements précis
// int_marker.controls.push_back(new_control);
//Boutton
// new_control.interaction_mode = InteractiveMarkerControl::BUTTON;
// new_control.name = "button_control";
// Marker marker = makeBox( int_marker );
// new_control.markers.push_back( marker );
// new_control.always_visible = true;
// int_marker.controls.push_back(new_control);
//TEST 6DOF
//int_marker.controls[0].interaction_mode = InteractiveMarkerControl::MOVE_ROTATE_3D; //Deplacement par "grab" de la souris (CTRL + Souris = rotation)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; //Ajout fonctionalité Boutton (Validation ?)
new_control.orientation_mode = InteractiveMarkerControl::FIXED;
//Orientation du vecteur
new_control.orientation.w = 1;
new_control.orientation.x = 1;
new_control.orientation.y = 0;
new_control.orientation.z = 0;
//Ajout des controles associées
new_control.name = "rotate_x";
new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(new_control);
new_control.name = "move_x";
new_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(new_control);
//Orientation du vecteur
new_control.orientation.w = 1;
new_control.orientation.x = 0;
new_control.orientation.y = 1;
new_control.orientation.z = 0;
//Ajout des controles associées
new_control.name = "rotate_y";
new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(new_control);
new_control.name = "move_y";
new_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(new_control);
//Orientation du vecteur
new_control.orientation.w = 1;
new_control.orientation.x = 0;
new_control.orientation.y = 0;
new_control.orientation.z = 1;
//Ajout des controles associées
new_control.name = "rotate_z";
new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(new_control);
new_control.name = "move_z";
new_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(new_control);
// add the interactive marker to our collection &
// tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, &processFeedback);
// 'commit' changes and send to all clients
server.applyChanges();
// start the ROS main loop
ros::spin();
// ros::Rate loop_rate(10);
// while (ros::ok())
// {
// objective_pub.publish(obj);
// ros::spinOnce();
// loop_rate.sleep();
// }
}
// %Tag(fullSource)%

172
rviz_interface/interface_tests/src/panel_test.cpp Normal file
View file

@ -0,0 +1,172 @@
#include "panel_test.hpp"
//#include <geometry_msgs/Twist.h>
namespace interface_tests
{
// BEGIN_TUTORIAL
// Here is the implementation of the TestPanel class. TestPanel
// has these responsibilities:
//
// - Act as a container for GUI elements DriveWidget and QLineEdit.
// - Publish command velocities 10 times per second (whether 0 or not).
// - Saving and restoring internal state from a config file.
//
// We start with the constructor, doing the standard Qt thing of
// passing the optional *parent* argument on to the superclass
// constructor, and also zero-ing the velocities we will be
// publishing.
TestPanel::TestPanel( QWidget* parent ): rviz::Panel( parent )
{
// Next we lay out the "output topic" text entry field using a
// QLabel and a QLineEdit in a QHBoxLayout.
QHBoxLayout* topic_layout = new QHBoxLayout;
topic_layout->addWidget( new QLabel( "Max Error:" ));
max_error_editor = new QLineEdit;
// max_error_editor->setValidator( new QDoubleValidator(0,MAX_ERROR,1000,max_error_editor)); // Ne gère pas les exceptions tout seul (retourne QValidator::Intermediate pour les valeurs hors bornes)
topic_layout->addWidget( max_error_editor );
setLayout( topic_layout );
// Then create the control widget.
// drive_widget_ = new DriveWidget;
// // Lay out the topic field above the control widget.
// QVBoxLayout* layout = new QVBoxLayout;
// layout->addLayout( topic_layout );
// layout->addWidget( drive_widget_ );
// setLayout( layout );
// Create a timer for sending the output. Motor controllers want to
// be reassured frequently that they are doing the right thing, so
// we keep re-sending velocities even when they aren't changing.
//
// Here we take advantage of QObject's memory management behavior:
// since "this" is passed to the new QTimer as its parent, the
// QTimer is deleted by the QObject destructor when this TestPanel
// object is destroyed. Therefore we don't need to keep a pointer
// to the timer.
// QTimer* output_timer = new QTimer( this );
// Next we make signal/slot connections.
// connect( drive_widget_, SIGNAL( outputVelocity( float, float )), this, SLOT( setVel( float, float )));
connect( max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() ));
// connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));
// Start the timer.
// output_timer->start( 100 );
// Make the control widget start disabled, since we don't start with an output topic.
// drive_widget_->setEnabled( false );
_config_publisher = nh_.advertise<interface_tests::InterfaceConfigT>( "interface_config", 1 );
}
// setVel() is connected to the DriveWidget's output, which is sent
// whenever it changes due to a mouse event. This just records the
// values it is given. The data doesn't actually get sent until the
// next timer callback.
// void TestPanel::setVel( float lin, float ang )
// {
// linear_velocity_ = lin;
// angular_velocity_ = ang;
// }
// Read the topic name from the QLineEdit and call setTopic() with the
// results. This is connected to QLineEdit::editingFinished() which
// fires when the user presses Enter or Tab or otherwise moves focus
// away.
void TestPanel::updateError()
{
//setTopic( max_error_editor->text() );
setError(max_error_editor->text());
}
void TestPanel::setError( const QString& error )
{
if( ros::ok() && _config_publisher )
{
interface_tests::InterfaceConfigT new_config;
new_config.max_error = error.toDouble();
_config_publisher.publish( new_config );
}
}
// Set the topic name we are publishing to.
// void TestPanel::setTopic( const QString& new_topic )
// {
// // Only take action if the name has changed.
// if( new_topic != output_topic_ )
// {
// output_topic_ = new_topic;
// // If the topic is the empty string, don't publish anything.
// if( output_topic_ == "" )
// {
// _config_publisher.shutdown();
// }
// else
// {
// // The old ``_config_publisher`` is destroyed by this assignment,
// // and thus the old topic advertisement is removed. The call to
// // nh_advertise() says we want to publish data on the new topic
// // name.
// _config_publisher = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );
// }
// // rviz::Panel defines the configChanged() signal. Emitting it
// // tells RViz that something in this panel has changed that will
// // affect a saved config file. Ultimately this signal can cause
// // QWidget::setWindowModified(true) to be called on the top-level
// // rviz::VisualizationFrame, which causes a little asterisk ("*")
// // to show in the window's title bar indicating unsaved changes.
// Q_EMIT configChanged();
// }
// // Gray out the control widget when the output topic is empty.
// // drive_widget_->setEnabled( output_topic_ != "" );
// }
// Publish the control velocities if ROS is not shutting down and the
// publisher is ready with a valid topic name.
// void TestPanel::sendVel()
// {
// if( ros::ok() && _config_publisher )
// {
// geometry_msgs::Twist msg;
// msg.linear.x = linear_velocity_;
// msg.linear.y = 0;
// msg.linear.z = 0;
// msg.angular.x = 0;
// msg.angular.y = 0;
// msg.angular.z = angular_velocity_;
// _config_publisher.publish( msg );
// }
// }
// Save all configuration data from this panel to the given
// Config object. It is important here that you call save()
// on the parent class so the class id and panel name get saved.
void TestPanel::save( rviz::Config config ) const
{
rviz::Panel::save( config );
config.mapSetValue( "Topic", output_topic_ );
}
// Load all configuration data for this panel from the given Config object.
void TestPanel::load( const rviz::Config& config )
{
rviz::Panel::load( config );
QString topic;
if( config.mapGetString( "Topic", &topic ))
{
max_error_editor->setText( topic );
updateError();
}
}
} // end namespace
// Tell pluginlib about this class. Every class which should be
// loadable by pluginlib::ClassLoader must have these two lines
// compiled in its .cpp file, outside of any namespace scope.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(interface_tests::TestPanel,rviz::Panel )

101
rviz_interface/interface_tests/src/panel_test.hpp Normal file
View file

@ -0,0 +1,101 @@
#ifndef PANEL_TEST_HPP
#define PANEL_TEST_HPP
//#ifndef Q_MOC_RUN
# include <ros/ros.h>
# include <rviz/panel.h>
//#endif
#include <stdio.h>
#include <QPainter>
#include <QLineEdit>
// #include <QDoubleValidator>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <interface_tests/InterfaceConfigT.h>
#define MAX_ERROR 999999999
class QLineEdit;
namespace interface_tests
{
// BEGIN_TUTORIAL
// Here we declare our new subclass of rviz::Panel. Every panel which
// can be added via the Panels/Add_New_Panel menu is a subclass of
// rviz::Panel.
//
class TestPanel: public rviz::Panel
{
// This class uses Qt slots and is a subclass of QObject, so it needs
// the Q_OBJECT macro.
Q_OBJECT
public:
// QWidget subclass constructors usually take a parent widget
// parameter (which usually defaults to 0). At the same time,
// pluginlib::ClassLoader creates instances by calling the default
// constructor (with no arguments). Taking the parameter and giving
// a default of 0 lets the default constructor work and also lets
// someone using the class for something else to pass in a parent
// widget as they normally would with Qt.
TestPanel( QWidget* parent = 0 );
// Now we declare overrides of rviz::Panel functions for saving and
// loading data from the config file. Here the data is the
// topic name.
virtual void load( const rviz::Config& config );
virtual void save( rviz::Config config ) const;
// Next come a couple of public Qt slots.
public Q_SLOTS:
// The control area, DriveWidget, sends its output to a Qt signal
// for ease of re-use, so here we declare a Qt slot to receive it.
// void setVel( float linear_velocity_, float angular_velocity_ );
// In this example setTopic() does not get connected to any signal
// (it is called directly), but it is easy to define it as a public
// slot instead of a private function in case it would be useful to
// some other user.
// void setTopic( const QString& topic );
void setError( const QString& error );
// Here we declare some internal slots.
protected Q_SLOTS:
// sendvel() publishes the current velocity values to a ROS
// topic. Internally this is connected to a timer which calls it 10
// times per second.
// void sendVel();
// updateError() reads the topic name from the QLineEdit and calls
// setTopic() with the result.
void updateError();
// Then we finish up with protected member variables.
protected:
// One-line text editor for entering the outgoing ROS topic name.
QLineEdit* max_error_editor;
// The current name of the output topic.
QString output_topic_;
// The ROS publisher for the command velocity.
ros::Publisher _config_publisher;
// The ROS node handle.
ros::NodeHandle nh_;
// The latest velocity values from the drive widget.
// float linear_velocity_;
// float angular_velocity_;
// END_TUTORIAL
};
} // end namespace rviz_plugin_tutorials
#endif