basic_controls.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include <ros/ros.h>
00032 
00033 #include <interactive_markers/interactive_marker_server.h>
00034 #include <interactive_markers/menu_handler.h>
00035 
00036 #include <tf/transform_broadcaster.h>
00037 #include <tf/tf.h>
00038 
00039 #include <math.h>
00040 
00041 using namespace visualization_msgs;
00042 
00043 
00044 // %Tag(vars)%
00045 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00046 interactive_markers::MenuHandler menu_handler;
00047 // %EndTag(vars)%
00048 
00049 
00050 // %Tag(Box)%
00051 Marker makeBox( InteractiveMarker &msg )
00052 {
00053   Marker marker;
00054 
00055   marker.type = Marker::CUBE;
00056   marker.scale.x = msg.scale * 0.45;
00057   marker.scale.y = msg.scale * 0.45;
00058   marker.scale.z = msg.scale * 0.45;
00059   marker.color.r = 0.5;
00060   marker.color.g = 0.5;
00061   marker.color.b = 0.5;
00062   marker.color.a = 1.0;
00063 
00064   return marker;
00065 }
00066 
00067 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00068 {
00069   InteractiveMarkerControl control;
00070   control.always_visible = true;
00071   control.markers.push_back( makeBox(msg) );
00072   msg.controls.push_back( control );
00073 
00074   return msg.controls.back();
00075 }
00076 // %EndTag(Box)%
00077 
00078 // %Tag(frameCallback)%
00079 void frameCallback(const ros::TimerEvent&)
00080 {
00081   static uint32_t counter = 0;
00082 
00083   static tf::TransformBroadcaster br;
00084 
00085   tf::Transform t;
00086 
00087   ros::Time time = ros::Time::now();
00088 
00089   t.setOrigin(tf::Vector3(0.0, 0.0, sin(float(counter)/140.0) * 2.0));
00090   t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00091   br.sendTransform(tf::StampedTransform(t, time, "base_link", "moving_frame"));
00092 
00093   t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00094   t.setRotation(tf::createQuaternionFromRPY(0.0, float(counter)/140.0, 0.0));
00095   br.sendTransform(tf::StampedTransform(t, time, "base_link", "rotating_frame"));
00096 
00097   counter++;
00098 }
00099 // %EndTag(frameCallback)%
00100 
00101 // %Tag(processFeedback)%
00102 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00103 {
00104   std::ostringstream s;
00105   s << "Feedback from marker '" << feedback->marker_name << "' "
00106       << " / control '" << feedback->control_name << "'";
00107 
00108   std::ostringstream mouse_point_ss;
00109   if( feedback->mouse_point_valid )
00110   {
00111     mouse_point_ss << " at " << feedback->mouse_point.x
00112                    << ", " << feedback->mouse_point.y
00113                    << ", " << feedback->mouse_point.z
00114                    << " in frame " << feedback->header.frame_id;
00115   }
00116 
00117   switch ( feedback->event_type )
00118   {
00119     case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
00120       ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
00121       break;
00122 
00123     case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
00124       ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
00125       break;
00126 
00127     case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00128       ROS_INFO_STREAM( s.str() << ": pose changed"
00129           << "\nposition = "
00130           << feedback->pose.position.x
00131           << ", " << feedback->pose.position.y
00132           << ", " << feedback->pose.position.z
00133           << "\norientation = "
00134           << feedback->pose.orientation.w
00135           << ", " << feedback->pose.orientation.x
00136           << ", " << feedback->pose.orientation.y
00137           << ", " << feedback->pose.orientation.z
00138           << "\nframe: " << feedback->header.frame_id
00139           << " time: " << feedback->header.stamp.sec << "sec, "
00140           << feedback->header.stamp.nsec << " nsec" );
00141       break;
00142 
00143     case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00144       ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
00145       break;
00146 
00147     case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00148       ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
00149       break;
00150   }
00151 
00152   server->applyChanges();
00153 }
00154 // %EndTag(processFeedback)%
00155 
00156 // %Tag(alignMarker)%
00157 void alignMarker( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00158 {
00159   geometry_msgs::Pose pose = feedback->pose;
00160 
00161   pose.position.x = round(pose.position.x-0.5)+0.5;
00162   pose.position.y = round(pose.position.y-0.5)+0.5;
00163 
00164   ROS_INFO_STREAM( feedback->marker_name << ":"
00165       << " aligning position = "
00166       << feedback->pose.position.x
00167       << ", " << feedback->pose.position.y
00168       << ", " << feedback->pose.position.z
00169       << " to "
00170       << pose.position.x
00171       << ", " << pose.position.y
00172       << ", " << pose.position.z );
00173 
00174   server->setPose( feedback->marker_name, pose );
00175   server->applyChanges();
00176 }
00177 // %EndTag(alignMarker)%
00178 
00179 double rand( double min, double max )
00180 {
00181   double t = (double)rand() / (double)RAND_MAX;
00182   return min + t*(max-min);
00183 }
00184 
00185 void saveMarker( InteractiveMarker int_marker )
00186 {
00187   server->insert(int_marker);
00188   server->setCallback(int_marker.name, &processFeedback);
00189 }
00190 
00192 
00193 // %Tag(6DOF)%
00194 void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector3& position, bool show_6dof )
00195 {
00196   InteractiveMarker int_marker;
00197   int_marker.header.frame_id = "base_link";
00198   tf::pointTFToMsg(position, int_marker.pose.position);
00199   int_marker.scale = 1;
00200 
00201   int_marker.name = "simple_6dof";
00202   int_marker.description = "Simple 6-DOF Control";
00203 
00204   // insert a box
00205   makeBoxControl(int_marker);
00206   int_marker.controls[0].interaction_mode = interaction_mode;
00207 
00208   InteractiveMarkerControl control;
00209 
00210   if ( fixed )
00211   {
00212     int_marker.name += "_fixed";
00213     int_marker.description += "\n(fixed orientation)";
00214     control.orientation_mode = InteractiveMarkerControl::FIXED;
00215   }
00216 
00217   if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE)
00218   {
00219       std::string mode_text;
00220       if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_3D )         mode_text = "MOVE_3D";
00221       if( interaction_mode == visualization_msgs::InteractiveMarkerControl::ROTATE_3D )       mode_text = "ROTATE_3D";
00222       if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D )  mode_text = "MOVE_ROTATE_3D";
00223       int_marker.name += "_" + mode_text;
00224       int_marker.description = std::string("3D Control") + (show_6dof ? " + 6-DOF controls" : "") + "\n" + mode_text;
00225   }
00226 
00227   if(show_6dof)
00228   {
00229     control.orientation.w = 1;
00230     control.orientation.x = 1;
00231     control.orientation.y = 0;
00232     control.orientation.z = 0;
00233     control.name = "rotate_x";
00234     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00235     int_marker.controls.push_back(control);
00236     control.name = "move_x";
00237     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00238     int_marker.controls.push_back(control);
00239 
00240     control.orientation.w = 1;
00241     control.orientation.x = 0;
00242     control.orientation.y = 1;
00243     control.orientation.z = 0;
00244     control.name = "rotate_z";
00245     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00246     int_marker.controls.push_back(control);
00247     control.name = "move_z";
00248     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00249     int_marker.controls.push_back(control);
00250 
00251     control.orientation.w = 1;
00252     control.orientation.x = 0;
00253     control.orientation.y = 0;
00254     control.orientation.z = 1;
00255     control.name = "rotate_y";
00256     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00257     int_marker.controls.push_back(control);
00258     control.name = "move_y";
00259     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00260     int_marker.controls.push_back(control);
00261   }
00262 
00263   server->insert(int_marker);
00264   server->setCallback(int_marker.name, &processFeedback);
00265   if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE)
00266     menu_handler.apply( *server, int_marker.name );
00267 }
00268 // %EndTag(6DOF)%
00269 
00270 // %Tag(RandomDof)%
00271 void makeRandomDofMarker( const tf::Vector3& position )
00272 {
00273   InteractiveMarker int_marker;
00274   int_marker.header.frame_id = "base_link";
00275   tf::pointTFToMsg(position, int_marker.pose.position);
00276   int_marker.scale = 1;
00277 
00278   int_marker.name = "6dof_random_axes";
00279   int_marker.description = "6-DOF\n(Arbitrary Axes)";
00280 
00281   makeBoxControl(int_marker);
00282 
00283   InteractiveMarkerControl control;
00284 
00285   for ( int i=0; i<3; i++ )
00286   {
00287     control.orientation.w = rand(-1,1);
00288     control.orientation.x = rand(-1,1);
00289     control.orientation.y = rand(-1,1);
00290     control.orientation.z = rand(-1,1);
00291     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00292     int_marker.controls.push_back(control);
00293     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00294     int_marker.controls.push_back(control);
00295   }
00296 
00297   server->insert(int_marker);
00298   server->setCallback(int_marker.name, &processFeedback);
00299 }
00300 // %EndTag(RandomDof)%
00301 
00302 
00303 // %Tag(ViewFacing)%
00304 void makeViewFacingMarker( const tf::Vector3& position )
00305 {
00306   InteractiveMarker int_marker;
00307   int_marker.header.frame_id = "base_link";
00308   tf::pointTFToMsg(position, int_marker.pose.position);
00309   int_marker.scale = 1;
00310 
00311   int_marker.name = "view_facing";
00312   int_marker.description = "View Facing 6-DOF";
00313 
00314   InteractiveMarkerControl control;
00315 
00316   // make a control that rotates around the view axis
00317   control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00318   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00319   control.orientation.w = 1;
00320   control.name = "rotate";
00321 
00322   int_marker.controls.push_back(control);
00323 
00324   // create a box in the center which should not be view facing,
00325   // but move in the camera plane.
00326   control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00327   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00328   control.independent_marker_orientation = true;
00329   control.name = "move";
00330 
00331   control.markers.push_back( makeBox(int_marker) );
00332   control.always_visible = true;
00333 
00334   int_marker.controls.push_back(control);
00335 
00336   server->insert(int_marker);
00337   server->setCallback(int_marker.name, &processFeedback);
00338 }
00339 // %EndTag(ViewFacing)%
00340 
00341 
00342 // %Tag(Quadrocopter)%
00343 void makeQuadrocopterMarker( const tf::Vector3& position )
00344 {
00345   InteractiveMarker int_marker;
00346   int_marker.header.frame_id = "base_link";
00347   tf::pointTFToMsg(position, int_marker.pose.position);
00348   int_marker.scale = 1;
00349 
00350   int_marker.name = "quadrocopter";
00351   int_marker.description = "Quadrocopter";
00352 
00353   makeBoxControl(int_marker);
00354 
00355   InteractiveMarkerControl control;
00356 
00357   control.orientation.w = 1;
00358   control.orientation.x = 0;
00359   control.orientation.y = 1;
00360   control.orientation.z = 0;
00361   control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00362   int_marker.controls.push_back(control);
00363   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00364   int_marker.controls.push_back(control);
00365 
00366   server->insert(int_marker);
00367   server->setCallback(int_marker.name, &processFeedback);
00368 }
00369 // %EndTag(Quadrocopter)%
00370 
00371 // %Tag(ChessPiece)%
00372 void makeChessPieceMarker( const tf::Vector3& position )
00373 {
00374   InteractiveMarker int_marker;
00375   int_marker.header.frame_id = "base_link";
00376   tf::pointTFToMsg(position, int_marker.pose.position);
00377   int_marker.scale = 1;
00378 
00379   int_marker.name = "chess_piece";
00380   int_marker.description = "Chess Piece\n(2D Move + Alignment)";
00381 
00382   InteractiveMarkerControl control;
00383 
00384   control.orientation.w = 1;
00385   control.orientation.x = 0;
00386   control.orientation.y = 1;
00387   control.orientation.z = 0;
00388   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00389   int_marker.controls.push_back(control);
00390 
00391   // make a box which also moves in the plane
00392   control.markers.push_back( makeBox(int_marker) );
00393   control.always_visible = true;
00394   int_marker.controls.push_back(control);
00395 
00396   // we want to use our special callback function
00397   server->insert(int_marker);
00398   server->setCallback(int_marker.name, &processFeedback);
00399 
00400   // set different callback for POSE_UPDATE feedback
00401   server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE );
00402 }
00403 // %EndTag(ChessPiece)%
00404 
00405 // %Tag(PanTilt)%
00406 void makePanTiltMarker( const tf::Vector3& position )
00407 {
00408   InteractiveMarker int_marker;
00409   int_marker.header.frame_id = "base_link";
00410   tf::pointTFToMsg(position, int_marker.pose.position);
00411   int_marker.scale = 1;
00412 
00413   int_marker.name = "pan_tilt";
00414   int_marker.description = "Pan / Tilt";
00415 
00416   makeBoxControl(int_marker);
00417 
00418   InteractiveMarkerControl control;
00419 
00420   control.orientation.w = 1;
00421   control.orientation.x = 0;
00422   control.orientation.y = 1;
00423   control.orientation.z = 0;
00424   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00425   control.orientation_mode = InteractiveMarkerControl::FIXED;
00426   int_marker.controls.push_back(control);
00427 
00428   control.orientation.w = 1;
00429   control.orientation.x = 0;
00430   control.orientation.y = 0;
00431   control.orientation.z = 1;
00432   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00433   control.orientation_mode = InteractiveMarkerControl::INHERIT;
00434   int_marker.controls.push_back(control);
00435 
00436   server->insert(int_marker);
00437   server->setCallback(int_marker.name, &processFeedback);
00438 }
00439 // %EndTag(PanTilt)%
00440 
00441 // %Tag(Menu)%
00442 void makeMenuMarker( const tf::Vector3& position )
00443 {
00444   InteractiveMarker int_marker;
00445   int_marker.header.frame_id = "base_link";
00446   tf::pointTFToMsg(position, int_marker.pose.position);
00447   int_marker.scale = 1;
00448 
00449   int_marker.name = "context_menu";
00450   int_marker.description = "Context Menu\n(Right Click)";
00451 
00452   InteractiveMarkerControl control;
00453 
00454   control.interaction_mode = InteractiveMarkerControl::MENU;
00455   control.name = "menu_only_control";
00456 
00457   Marker marker = makeBox( int_marker );
00458   control.markers.push_back( marker );
00459   control.always_visible = true;
00460   int_marker.controls.push_back(control);
00461 
00462   server->insert(int_marker);
00463   server->setCallback(int_marker.name, &processFeedback);
00464   menu_handler.apply( *server, int_marker.name );
00465 }
00466 // %EndTag(Menu)%
00467 
00468 // %Tag(Button)%
00469 void makeButtonMarker( const tf::Vector3& position )
00470 {
00471   InteractiveMarker int_marker;
00472   int_marker.header.frame_id = "base_link";
00473   tf::pointTFToMsg(position, int_marker.pose.position);
00474   int_marker.scale = 1;
00475 
00476   int_marker.name = "button";
00477   int_marker.description = "Button\n(Left Click)";
00478 
00479   InteractiveMarkerControl control;
00480 
00481   control.interaction_mode = InteractiveMarkerControl::BUTTON;
00482   control.name = "button_control";
00483 
00484   Marker marker = makeBox( int_marker );
00485   control.markers.push_back( marker );
00486   control.always_visible = true;
00487   int_marker.controls.push_back(control);
00488 
00489   server->insert(int_marker);
00490   server->setCallback(int_marker.name, &processFeedback);
00491 }
00492 // %EndTag(Button)%
00493 
00494 // %Tag(Moving)%
00495 void makeMovingMarker( const tf::Vector3& position )
00496 {
00497   InteractiveMarker int_marker;
00498   int_marker.header.frame_id = "moving_frame";
00499   tf::pointTFToMsg(position, int_marker.pose.position);
00500   int_marker.scale = 1;
00501 
00502   int_marker.name = "moving";
00503   int_marker.description = "Marker Attached to a\nMoving Frame";
00504 
00505   InteractiveMarkerControl control;
00506 
00507   control.orientation.w = 1;
00508   control.orientation.x = 1;
00509   control.orientation.y = 0;
00510   control.orientation.z = 0;
00511   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00512   int_marker.controls.push_back(control);
00513 
00514   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00515   control.always_visible = true;
00516   control.markers.push_back( makeBox(int_marker) );
00517   int_marker.controls.push_back(control);
00518 
00519   server->insert(int_marker);
00520   server->setCallback(int_marker.name, &processFeedback);
00521 }
00522 // %EndTag(Moving)%
00523 
00524 // %Tag(main)%
00525 int main(int argc, char** argv)
00526 {
00527   ros::init(argc, argv, "basic_controls");
00528   ros::NodeHandle n;
00529 
00530   // create a timer to update the published transforms
00531   ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
00532 
00533   server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) );
00534 
00535   ros::Duration(0.1).sleep();
00536 
00537   menu_handler.insert( "First Entry", &processFeedback );
00538   menu_handler.insert( "Second Entry", &processFeedback );
00539   interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );
00540   menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
00541   menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );
00542 
00543   tf::Vector3 position;
00544   position = tf::Vector3(-3, 3, 0);
00545   make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::NONE, position, true );
00546   position = tf::Vector3( 0, 3, 0);
00547   make6DofMarker( true, visualization_msgs::InteractiveMarkerControl::NONE, position, true );
00548   position = tf::Vector3( 3, 3, 0);
00549   makeRandomDofMarker( position );
00550   position = tf::Vector3(-3, 0, 0);
00551   make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::ROTATE_3D, position, false );
00552   position = tf::Vector3( 0, 0, 0);
00553   make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D, position, true );
00554   position = tf::Vector3( 3, 0, 0);
00555   make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_3D, position, false );
00556   position = tf::Vector3(-3,-3, 0);
00557   makeViewFacingMarker( position );
00558   position = tf::Vector3( 0,-3, 0);
00559   makeQuadrocopterMarker( position );
00560   position = tf::Vector3( 3,-3, 0);
00561   makeChessPieceMarker( position );
00562   position = tf::Vector3(-3,-6, 0);
00563   makePanTiltMarker( position );
00564   position = tf::Vector3( 0,-6, 0);
00565   makeMovingMarker( position );
00566   position = tf::Vector3( 3,-6, 0);
00567   makeMenuMarker( position );
00568   position = tf::Vector3( 0,-9, 0);
00569   makeButtonMarker( position );
00570 
00571   server->applyChanges();
00572 
00573   ros::spin();
00574 
00575   server.reset();
00576 }
00577 // %EndTag(main)%


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Mon Oct 6 2014 08:46:33