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 float marker_pos = 0;
00047 interactive_markers::MenuHandler menu_handler;
00048 // %EndTag(vars)%
00049 
00050 
00051 // %Tag(Box)%
00052 Marker makeBox( InteractiveMarker &msg )
00053 {
00054   Marker marker;
00055 
00056   marker.type = Marker::CUBE;
00057   marker.scale.x = msg.scale * 0.45;
00058   marker.scale.y = msg.scale * 0.45;
00059   marker.scale.z = msg.scale * 0.45;
00060   marker.color.r = 0.5;
00061   marker.color.g = 0.5;
00062   marker.color.b = 0.5;
00063   marker.color.a = 1.0;
00064 
00065   return marker;
00066 }
00067 
00068 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00069 {
00070   InteractiveMarkerControl control;
00071   control.always_visible = true;
00072   control.markers.push_back( makeBox(msg) );
00073   msg.controls.push_back( control );
00074 
00075   return msg.controls.back();
00076 }
00077 // %EndTag(Box)%
00078 
00079 // %Tag(frameCallback)%
00080 void frameCallback(const ros::TimerEvent&)
00081 {
00082   static uint32_t counter = 0;
00083   static bool make = true;
00084 
00085   static tf::TransformBroadcaster br;
00086 
00087   tf::Transform t;
00088 
00089   ros::Time time = ros::Time::now();
00090 
00091   t.setOrigin(tf::Vector3(0.0, 0.0, sin(float(counter)/140.0) * 2.0));
00092   t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00093   br.sendTransform(tf::StampedTransform(t, time, "base_link", "moving_frame"));
00094 
00095   t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00096   t.setRotation(tf::createQuaternionFromRPY(0.0, float(counter)/140.0, 0.0));
00097   br.sendTransform(tf::StampedTransform(t, time, "base_link", "rotating_frame"));
00098 
00099   ++counter;
00100   if( (counter % 400) == 0 )
00101   {
00102     if( make )
00103     {
00104       InteractiveMarker int_marker;
00105       int_marker.header.frame_id = "/base_link";
00106       int_marker.pose.position.x = 5;
00107       int_marker.scale = 1;
00108 
00109       int_marker.name = "blinky";
00110       int_marker.description = "blinking 6-DOF Control";
00111 
00112       {
00113         InteractiveMarkerControl control;
00114 
00115         control.interaction_mode = InteractiveMarkerControl::MENU;
00116         control.description="Blinky Options";
00117         control.name = "menu_only_control";
00118         control.always_visible = true;
00119 
00120         Marker marker = makeBox( int_marker );
00121         control.markers.push_back( marker );
00122 
00123         int_marker.controls.push_back(control);
00124       }
00125 
00126       InteractiveMarkerControl control;
00127 
00128       control.orientation.w = 1;
00129       control.orientation.x = 1;
00130       control.orientation.y = 0;
00131       control.orientation.z = 0;
00132       control.name = "rotate_x";
00133       control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00134       int_marker.controls.push_back(control);
00135       control.name = "move_x";
00136       control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00137       int_marker.controls.push_back(control);
00138 
00139       control.orientation.w = 1;
00140       control.orientation.x = 0;
00141       control.orientation.y = 1;
00142       control.orientation.z = 0;
00143       control.name = "rotate_z";
00144       control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00145       int_marker.controls.push_back(control);
00146       control.name = "move_z";
00147       control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00148       int_marker.controls.push_back(control);
00149 
00150       control.orientation.w = 1;
00151       control.orientation.x = 0;
00152       control.orientation.y = 0;
00153       control.orientation.z = 1;
00154       control.name = "rotate_y";
00155       control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00156       int_marker.controls.push_back(control);
00157       control.name = "move_y";
00158       control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00159       int_marker.controls.push_back(control);
00160 
00161       server->insert(int_marker);
00162       menu_handler.apply( *server, int_marker.name );
00163 
00164       server->applyChanges();
00165       make = false;
00166     }
00167     else
00168     {
00169       server->erase( "blinky" );
00170       server->applyChanges();
00171       make = true;
00172     }
00173   }
00174 }
00175 // %EndTag(frameCallback)%
00176 
00177 // %Tag(processFeedback)%
00178 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00179 {
00180   std::ostringstream s;
00181   s << "Feedback from marker '" << feedback->marker_name << "' "
00182       << " / control '" << feedback->control_name << "'";
00183 
00184   std::ostringstream mouse_point_ss;
00185   if( feedback->mouse_point_valid )
00186   {
00187     mouse_point_ss << " at " << feedback->mouse_point.x
00188                    << ", " << feedback->mouse_point.y
00189                    << ", " << feedback->mouse_point.z
00190                    << " in frame " << feedback->header.frame_id;
00191   }
00192 
00193   switch ( feedback->event_type )
00194   {
00195     case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
00196       ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
00197       break;
00198 
00199     case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
00200       ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
00201       break;
00202 
00203     case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00204       ROS_INFO_STREAM( s.str() << ": pose changed"
00205           << "\nposition = "
00206           << feedback->pose.position.x
00207           << ", " << feedback->pose.position.y
00208           << ", " << feedback->pose.position.z
00209           << "\norientation = "
00210           << feedback->pose.orientation.w
00211           << ", " << feedback->pose.orientation.x
00212           << ", " << feedback->pose.orientation.y
00213           << ", " << feedback->pose.orientation.z
00214           << "\nframe: " << feedback->header.frame_id
00215           << " time: " << feedback->header.stamp.sec << "sec, "
00216           << feedback->header.stamp.nsec << " nsec" );
00217       break;
00218 
00219     case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00220       ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
00221       break;
00222 
00223     case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00224       ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
00225       break;
00226   }
00227 
00228   server->applyChanges();
00229 }
00230 // %EndTag(processFeedback)%
00231 
00232 // %Tag(alignMarker)%
00233 void alignMarker( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00234 {
00235   geometry_msgs::Pose pose = feedback->pose;
00236 
00237   pose.position.x = round(pose.position.x-0.5)+0.5;
00238   pose.position.y = round(pose.position.y-0.5)+0.5;
00239 
00240   ROS_INFO_STREAM( feedback->marker_name << ":"
00241       << " aligning position = "
00242       << feedback->pose.position.x
00243       << ", " << feedback->pose.position.y
00244       << ", " << feedback->pose.position.z
00245       << " to "
00246       << pose.position.x
00247       << ", " << pose.position.y
00248       << ", " << pose.position.z );
00249 
00250   server->setPose( feedback->marker_name, pose );
00251   server->applyChanges();
00252 }
00253 // %EndTag(alignMarker)%
00254 
00255 double rand( double min, double max )
00256 {
00257   double t = (double)rand() / (double)RAND_MAX;
00258   return min + t*(max-min);
00259 }
00260 
00261 void saveMarker( InteractiveMarker int_marker )
00262 {
00263   server->insert(int_marker);
00264   server->setCallback(int_marker.name, &processFeedback);
00265 }
00266 
00268 
00269 // %Tag(6DOF)%
00270 void make6DofMarker( bool fixed )
00271 {
00272   InteractiveMarker int_marker;
00273   int_marker.header.frame_id = "/base_link";
00274   int_marker.pose.position.y = -3.0 * marker_pos++;;
00275   int_marker.scale = 1;
00276 
00277   int_marker.name = "simple_6dof";
00278   int_marker.description = "Simple 6-DOF Control";
00279 
00280   // insert a box
00281   makeBoxControl(int_marker);
00282 
00283   InteractiveMarkerControl control;
00284 
00285   if ( fixed )
00286   {
00287     int_marker.name += "_fixed";
00288     int_marker.description += "\n(fixed orientation)";
00289     control.orientation_mode = InteractiveMarkerControl::FIXED;
00290   }
00291 
00292   control.orientation.w = 1;
00293   control.orientation.x = 1;
00294   control.orientation.y = 0;
00295   control.orientation.z = 0;
00296   control.name = "rotate_x";
00297   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00298   int_marker.controls.push_back(control);
00299   control.name = "move_x";
00300   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00301   int_marker.controls.push_back(control);
00302 
00303   control.orientation.w = 1;
00304   control.orientation.x = 0;
00305   control.orientation.y = 1;
00306   control.orientation.z = 0;
00307   control.name = "rotate_z";
00308   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00309   int_marker.controls.push_back(control);
00310   control.name = "move_z";
00311   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00312   int_marker.controls.push_back(control);
00313 
00314   control.orientation.w = 1;
00315   control.orientation.x = 0;
00316   control.orientation.y = 0;
00317   control.orientation.z = 1;
00318   control.name = "rotate_y";
00319   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00320   int_marker.controls.push_back(control);
00321   control.name = "move_y";
00322   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00323   int_marker.controls.push_back(control);
00324 
00325   server->insert(int_marker);
00326   server->setCallback(int_marker.name, &processFeedback);
00327 }
00328 // %EndTag(6DOF)%
00329 
00330 // %Tag(RandomDof)%
00331 void makeRandomDofMarker( )
00332 {
00333   InteractiveMarker int_marker;
00334   int_marker.header.frame_id = "/base_link";
00335   int_marker.pose.position.y = -3.0 * marker_pos++;;
00336   int_marker.scale = 1;
00337 
00338   int_marker.name = "6dof_random_axes";
00339   int_marker.description = "6-DOF\n(Arbitrary Axes)";
00340 
00341   makeBoxControl(int_marker);
00342 
00343   InteractiveMarkerControl control;
00344 
00345   for ( int i=0; i<3; i++ )
00346   {
00347     control.orientation.w = rand(-1,1);
00348     control.orientation.x = rand(-1,1);
00349     control.orientation.y = rand(-1,1);
00350     control.orientation.z = rand(-1,1);
00351     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00352     int_marker.controls.push_back(control);
00353     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00354     int_marker.controls.push_back(control);
00355   }
00356 
00357   server->insert(int_marker);
00358   server->setCallback(int_marker.name, &processFeedback);
00359 }
00360 // %EndTag(RandomDof)%
00361 
00362 
00363 // %Tag(ViewFacing)%
00364 void makeViewFacingMarker( )
00365 {
00366   InteractiveMarker int_marker;
00367   int_marker.header.frame_id = "/base_link";
00368   int_marker.pose.position.y = -3.0 * marker_pos++;;
00369   int_marker.scale = 1;
00370 
00371   int_marker.name = "view_facing";
00372   int_marker.description = "View Facing 6-DOF";
00373 
00374   InteractiveMarkerControl control;
00375 
00376   // make a control that rotates around the view axis
00377   control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00378   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00379   control.orientation.w = 1;
00380   control.name = "rotate";
00381 
00382   int_marker.controls.push_back(control);
00383 
00384   // create a box in the center which should not be view facing,
00385   // but move in the camera plane.
00386   control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00387   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00388   control.independent_marker_orientation = true;
00389   control.name = "move";
00390 
00391   control.markers.push_back( makeBox(int_marker) );
00392   control.always_visible = true;
00393 
00394   int_marker.controls.push_back(control);
00395 
00396   server->insert(int_marker);
00397   server->setCallback(int_marker.name, &processFeedback);
00398 }
00399 // %EndTag(ViewFacing)%
00400 
00401 
00402 // %Tag(Quadrocopter)%
00403 void makeQuadrocopterMarker( )
00404 {
00405   InteractiveMarker int_marker;
00406   int_marker.header.frame_id = "/base_link";
00407   int_marker.pose.position.y = -3.0 * marker_pos++;;
00408   int_marker.scale = 1;
00409 
00410   int_marker.name = "quadrocopter";
00411   int_marker.description = "Quadrocopter";
00412 
00413   makeBoxControl(int_marker);
00414 
00415   InteractiveMarkerControl control;
00416 
00417   control.orientation.w = 1;
00418   control.orientation.x = 0;
00419   control.orientation.y = 1;
00420   control.orientation.z = 0;
00421   control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00422   int_marker.controls.push_back(control);
00423   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00424   int_marker.controls.push_back(control);
00425 
00426   server->insert(int_marker);
00427   server->setCallback(int_marker.name, &processFeedback);
00428 }
00429 // %EndTag(Quadrocopter)%
00430 
00431 // %Tag(ChessPiece)%
00432 void makeChessPieceMarker( )
00433 {
00434   InteractiveMarker int_marker;
00435   int_marker.header.frame_id = "/base_link";
00436   int_marker.pose.position.y = -3.0 * marker_pos++;;
00437   int_marker.scale = 1;
00438 
00439   int_marker.name = "chess_piece";
00440   int_marker.description = "Chess Piece\n(2D Move + Alignment)";
00441 
00442   InteractiveMarkerControl control;
00443 
00444   control.orientation.w = 1;
00445   control.orientation.x = 0;
00446   control.orientation.y = 1;
00447   control.orientation.z = 0;
00448   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00449   int_marker.controls.push_back(control);
00450 
00451   // make a box which also moves in the plane
00452   control.markers.push_back( makeBox(int_marker) );
00453   control.always_visible = true;
00454   int_marker.controls.push_back(control);
00455 
00456   // we want to use our special callback function
00457   server->insert(int_marker);
00458   server->setCallback(int_marker.name, &processFeedback);
00459 
00460   // set different callback for POSE_UPDATE feedback
00461   server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE );
00462 }
00463 // %EndTag(ChessPiece)%
00464 
00465 // %Tag(PanTilt)%
00466 void makePanTiltMarker( )
00467 {
00468   InteractiveMarker int_marker;
00469   int_marker.header.frame_id = "/base_link";
00470   int_marker.pose.position.y = -3.0 * marker_pos++;;
00471   int_marker.scale = 1;
00472 
00473   int_marker.name = "pan_tilt";
00474   int_marker.description = "Pan / Tilt";
00475 
00476   makeBoxControl(int_marker);
00477 
00478   InteractiveMarkerControl control;
00479 
00480   control.orientation.w = 1;
00481   control.orientation.x = 0;
00482   control.orientation.y = 1;
00483   control.orientation.z = 0;
00484   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00485   control.orientation_mode = InteractiveMarkerControl::FIXED;
00486   int_marker.controls.push_back(control);
00487 
00488   control.orientation.w = 1;
00489   control.orientation.x = 0;
00490   control.orientation.y = 0;
00491   control.orientation.z = 1;
00492   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00493   control.orientation_mode = InteractiveMarkerControl::INHERIT;
00494   int_marker.controls.push_back(control);
00495 
00496   server->insert(int_marker);
00497   server->setCallback(int_marker.name, &processFeedback);
00498 }
00499 // %EndTag(PanTilt)%
00500 
00501 // %Tag(Menu)%
00502 void makeMenuMarker()
00503 {
00504   InteractiveMarker int_marker;
00505   int_marker.header.frame_id = "/base_link";
00506   int_marker.pose.position.y = -3.0 * marker_pos++;;
00507   int_marker.scale = 1;
00508 
00509   int_marker.name = "context_menu";
00510   int_marker.description = "Context Menu\n(Right Click)";
00511 
00512   InteractiveMarkerControl control;
00513 
00514   control.interaction_mode = InteractiveMarkerControl::MENU;
00515   control.description="Options";
00516   control.name = "menu_only_control";
00517 
00518   Marker marker = makeBox( int_marker );
00519   control.markers.push_back( marker );
00520   control.always_visible = true;
00521   int_marker.controls.push_back(control);
00522 
00523   server->insert(int_marker);
00524   server->setCallback(int_marker.name, &processFeedback);
00525   menu_handler.apply( *server, int_marker.name );
00526 }
00527 // %EndTag(Menu)%
00528 
00529 // %Tag(Moving)%
00530 void makeMovingMarker()
00531 {
00532   InteractiveMarker int_marker;
00533   int_marker.header.frame_id = "/moving_frame";
00534   int_marker.pose.position.y = -3.0 * marker_pos++;;
00535   int_marker.scale = 1;
00536 
00537   int_marker.name = "moving";
00538   int_marker.description = "Marker Attached to a\nMoving Frame";
00539 
00540   InteractiveMarkerControl control;
00541 
00542   control.orientation.w = 1;
00543   control.orientation.x = 1;
00544   control.orientation.y = 0;
00545   control.orientation.z = 0;
00546   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00547   int_marker.controls.push_back(control);
00548 
00549   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00550   control.always_visible = true;
00551   control.markers.push_back( makeBox(int_marker) );
00552   int_marker.controls.push_back(control);
00553 
00554   server->insert(int_marker);
00555   server->setCallback(int_marker.name, &processFeedback);
00556 }
00557 // %EndTag(Moving)%
00558 
00559 // %Tag(main)%
00560 int main(int argc, char** argv)
00561 {
00562   ros::init(argc, argv, "basic_controls");
00563   ros::NodeHandle n;
00564 
00565   // create a timer to update the published transforms
00566   ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
00567 
00568   server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) );
00569 
00570   ros::Duration(0.1).sleep();
00571 
00572   menu_handler.insert( "First Entry", &processFeedback );
00573   menu_handler.insert( "Second Entry", &processFeedback );
00574   interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );
00575   menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
00576   menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );
00577 
00578   make6DofMarker( false );
00579   make6DofMarker( true );
00580   makeRandomDofMarker( );
00581   makeViewFacingMarker( );
00582   makeQuadrocopterMarker( );
00583   makeChessPieceMarker( );
00584   makePanTiltMarker( );
00585   makeMenuMarker( );
00586   makeMovingMarker( );
00587 
00588   server->applyChanges();
00589 
00590   ros::spin();
00591 
00592   server.reset();
00593 }
00594 // %EndTag(main)%


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Sat Dec 28 2013 17:47:29