Rangement
This commit is contained in:
Unknown
parent
5ea3bd93c8
commit
97b12ef01e
43 changed files with 9 additions and 301 deletions
292
ongoing/interface_tests/src/marker_test.cpp
Normal file
292
ongoing/interface_tests/src/marker_test.cpp
Normal 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)%
|
||||
Loading…
Add table
Add a link
Reference in a new issue