rviz: rviz: interactive_marker_test.cpp Source File

00001 #include "ros/ros.h"
00002 
00003 #include <visualization_msgs/InteractiveMarkerArray.h>
00004 
00005 #include <tf/transform_broadcaster.h>
00006 #include <tf/tf.h>
00007 
00008 #include <math.h>
00009 
00010 using namespace visualization_msgs;
00011 
00012 InteractiveMarkerArray int_marker_array;
00013 float marker_pos = 0;
00014 
00016 
00017 void frameCallback(const ros::TimerEvent&)
00018 {
00019   static uint32_t counter = 0;
00020 
00021   static tf::TransformBroadcaster br;
00022 
00023   tf::Transform t;
00024 
00025   t.setOrigin(tf::Vector3(0.0, 0.0, sin(float(counter)/70.0) * 1));
00026   t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00027   br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "base_link", "moving_frame"));
00028 
00029   t.setOrigin(tf::Vector3(0.0, 0.0, 10.0));
00030   t.setRotation(tf::createQuaternionFromRPY(0.0, 0.0, M_PI*0.2));
00031   br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "moving_frame", "move_rotate_frame"));
00032 
00033   ++counter;
00034 }
00035 
00036 double rand( double min, double max )
00037 {
00038   double t = (double)rand() / (double)RAND_MAX;
00039   return min + t*(max-min);
00040 }
00041 
00042 
00043 Marker makeBox( InteractiveMarker &msg )
00044 {
00045   Marker marker;
00046 
00047   marker.type = Marker::CUBE;
00048   marker.scale.x = msg.scale * 0.45;
00049   marker.scale.y = msg.scale * 0.45;
00050   marker.scale.z = msg.scale * 0.45;
00051   marker.color.r = 1.0;
00052   marker.color.g = 1.0;
00053   marker.color.b = 1.0;
00054   marker.color.a = 1.0;
00055 
00056   return marker;
00057 }
00058 
00059 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00060 {
00061   InteractiveMarkerControl control;
00062   control.always_visible = true;
00063   control.markers.push_back( makeBox(msg) );
00064   msg.controls.push_back( control );
00065 
00066   return msg.controls.back();
00067 }
00068 
00069 InteractiveMarker makeEmptyMarker( bool dummyBox=true )
00070 {
00071   std_msgs::Header header;
00072   header.frame_id = "/base_link";
00073 //  header.stamp = ros::Time::now();
00074 
00075   geometry_msgs::Pose pose;
00076   pose.orientation.w = 1.0;
00077   pose.position.y = -3.0 * marker_pos++;
00078   pose.position.x = -2.0;
00079 
00080   InteractiveMarker int_marker;
00081   int_marker.header = header;
00082   int_marker.pose = pose;
00083   int_marker.scale = 1.0;
00084   int_marker.frame_locked = true;
00085 
00086   return int_marker;
00087 }
00088 
00089 void saveMarker( InteractiveMarker int_marker )
00090 {
00091   int_marker_array.markers.push_back(int_marker);
00092 }
00093 
00095 
00096 
00097 void make6DofMarker( bool fixed )
00098 {
00099   InteractiveMarker int_marker = makeEmptyMarker();
00100 
00101   int_marker.name = "Simple 6-DOF Control";
00102 
00103   int_marker.scale = 1.0;
00104   int_marker.header.frame_id = "/move_rotate_frame";
00105 
00106   makeBoxControl(int_marker);
00107 
00108   InteractiveMarkerControl control;
00109 
00110   if ( fixed )
00111   {
00112     int_marker.name += "\n(fixed orientation)";
00113     control.orientation_mode = InteractiveMarkerControl::FIXED;
00114   }
00115 
00116   control.orientation.w = 1;
00117   control.orientation.x = 1;
00118   control.orientation.y = 0;
00119   control.orientation.z = 0;
00120   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00121   int_marker.controls.push_back(control);
00122   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00123 //  int_marker.controls.push_back(control);
00124 
00125   control.orientation.w = 1;
00126   control.orientation.x = 0;
00127   control.orientation.y = 1;
00128   control.orientation.z = 0;
00129   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00130   int_marker.controls.push_back(control);
00131   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00132 //  int_marker.controls.push_back(control);
00133 
00134   control.orientation.w = 1;
00135   control.orientation.x = 0;
00136   control.orientation.y = 0;
00137   control.orientation.z = 1;
00138   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00139   int_marker.controls.push_back(control);
00140   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00141 //  int_marker.controls.push_back(control);
00142 
00143   saveMarker( int_marker );
00144 }
00145 
00146 void makeRandomDofMarker( )
00147 {
00148   InteractiveMarker int_marker = makeEmptyMarker();
00149   int_marker.name = "6-DOF\n(Arbitrary Axes)";
00150   int_marker.header.frame_id = "/move_rotate_frame";
00151 
00152   makeBoxControl(int_marker);
00153 
00154   InteractiveMarkerControl control;
00155 
00156   for ( int i=0; i<3; i++ )
00157   {
00158     control.orientation.w = rand(-1,1);
00159     control.orientation.x = rand(-1,1);
00160     control.orientation.y = rand(-1,1);
00161     control.orientation.z = rand(-1,1);
00162     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00163     int_marker.controls.push_back(control);
00164     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00165     int_marker.controls.push_back(control);
00166   }
00167 
00168   saveMarker( int_marker );
00169 }
00170 
00171 
00172 void makeViewFacingMarker( )
00173 {
00174   InteractiveMarker int_marker = makeEmptyMarker();
00175   int_marker.name = "View Facing 6-DOF";
00176   int_marker.header.frame_id = "/move_rotate_frame";
00177 
00178   makeBoxControl(int_marker);
00179 
00180   InteractiveMarkerControl control;
00181 
00182   control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00183   control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00184   int_marker.controls.push_back(control);
00185 
00186   saveMarker( int_marker );
00187 }
00188 
00189 
00190 void makeQuadrocopterMarker( )
00191 {
00192   InteractiveMarker int_marker = makeEmptyMarker();
00193   int_marker.name = "Quadrocopter Mode\n(Dog Walk + Elevation)";
00194   int_marker.header.frame_id = "/move_rotate_frame";
00195 
00196   makeBoxControl(int_marker);
00197 
00198   InteractiveMarkerControl control;
00199 
00200   control.orientation.w = 1;
00201   control.orientation.x = 0;
00202   control.orientation.y = 1;
00203   control.orientation.z = 0;
00204   control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00205   int_marker.controls.push_back(control);
00206   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00207   int_marker.controls.push_back(control);
00208 
00209   saveMarker( int_marker );
00210 }
00211 
00212 void makeChessPieceMarker( )
00213 {
00214   InteractiveMarker int_marker = makeEmptyMarker();
00215   int_marker.name = "Chess Piece\n(2D move)";
00216   int_marker.header.frame_id = "/move_rotate_frame";
00217 
00218   InteractiveMarkerControl control;
00219 
00220   control.orientation.w = 1;
00221   control.orientation.x = 0;
00222   control.orientation.y = 1;
00223   control.orientation.z = 0;
00224   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00225   int_marker.controls.push_back(control);
00226 
00227   // make a box which also moves in the plane
00228   control.markers.push_back( makeBox(int_marker) );
00229   control.always_visible = true;
00230   int_marker.controls.push_back(control);
00231 
00232   saveMarker( int_marker );
00233 }
00234 
00235 void makePanTiltMarker( )
00236 {
00237   InteractiveMarker int_marker = makeEmptyMarker();
00238   int_marker.name = "Pan / Tilt";
00239   int_marker.header.frame_id = "/move_rotate_frame";
00240 
00241   makeBoxControl(int_marker);
00242 
00243   InteractiveMarkerControl control;
00244 
00245   control.orientation.w = 1;
00246   control.orientation.x = 0;
00247   control.orientation.y = 1;
00248   control.orientation.z = 0;
00249   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00250   control.orientation_mode = InteractiveMarkerControl::FIXED;
00251   int_marker.controls.push_back(control);
00252 
00253   control.orientation.w = 1;
00254   control.orientation.x = 0;
00255   control.orientation.y = 0;
00256   control.orientation.z = 1;
00257   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00258   control.orientation_mode = InteractiveMarkerControl::INHERIT;
00259   int_marker.controls.push_back(control);
00260 
00261   saveMarker( int_marker );
00262 }
00263 
00264 void makeMenuMarker()
00265 {
00266   InteractiveMarker int_marker = makeEmptyMarker();
00267   int_marker.name = "Context Menu\n(Right Click)";
00268   int_marker.header.frame_id = "/move_rotate_frame";
00269 
00270   InteractiveMarkerControl control;
00271 
00272   control.orientation.w = 1;
00273   control.orientation.x = 0;
00274   control.orientation.y = 1;
00275   control.orientation.z = 0;
00276   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00277   int_marker.controls.push_back(control);
00278 
00279   control.interaction_mode = InteractiveMarkerControl::MENU;
00280   control.always_visible = true;
00281   control.markers.push_back( makeBox(int_marker) );
00282   int_marker.controls.push_back(control);
00283 
00284   visualization_msgs::Menu menu;
00285 
00286   menu.title = "First Entry";
00287   int_marker.menu.push_back( menu );
00288 
00289   menu.title = "Second Entry";
00290   int_marker.menu.push_back( menu );
00291 
00292   menu.title = "Submenu";
00293   menu.entries.push_back("First Submenu Entry");
00294   menu.entries.push_back("Second Submenu Entry");
00295   int_marker.menu.push_back( menu );
00296 
00297   saveMarker( int_marker );
00298 }
00299 
00300 
00301 int main(int argc, char** argv)
00302 {
00303   ros::init(argc, argv, "interactive_marker_test");
00304   ros::NodeHandle n;
00305 
00306   ros::Publisher marker_pub;
00307   marker_pub = n.advertise<InteractiveMarkerArray> ("interactive_marker_array", 0, true);
00308 //  ros::Timer publish_timer = n.createTimer(ros::Duration(1), publishCallback);
00309   ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
00310 
00311 //  srand( ros::Time::now().sec );
00312 
00313   tf::TransformBroadcaster tf_broadcaster;
00314 
00315   ros::Duration(0.1).sleep();
00316 
00317   make6DofMarker( false );
00318   make6DofMarker( true );
00319   makeRandomDofMarker( );
00320   makeViewFacingMarker( );
00321   makeQuadrocopterMarker( );
00322   makeChessPieceMarker( );
00323   makePanTiltMarker( );
00324   makeMenuMarker( );
00325 
00326   ROS_INFO( "Publishing %d markers", (int)int_marker_array.markers.size() );
00327   marker_pub.publish( int_marker_array );
00328 
00329   ros::spin();
00330 }