Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <ros/ros.h>
00033
00034 #include <interactive_markers/interactive_marker_server.h>
00035 #include <interactive_markers/menu_handler.h>
00036
00037
00038 interactive_markers::InteractiveMarkerServer* server;
00039
00040 visualization_msgs::InteractiveMarker makeMarker( float r, float g, float b )
00041 {
00042
00043 visualization_msgs::InteractiveMarker int_marker;
00044 int_marker.header.frame_id = "/base_link";
00045 int_marker.name = "my_marker";
00046 int_marker.description = "Simple 1-DOF Control";
00047
00048
00049 visualization_msgs::Marker box_marker;
00050 box_marker.type = visualization_msgs::Marker::CUBE;
00051 box_marker.scale.x = 0.45;
00052 box_marker.scale.y = 0.45;
00053 box_marker.scale.z = 0.45;
00054 box_marker.color.r = r;
00055 box_marker.color.g = g;
00056 box_marker.color.b = b;
00057 box_marker.color.a = 1.0;
00058
00059
00060 visualization_msgs::InteractiveMarkerControl box_control;
00061 box_control.always_visible = true;
00062 box_control.markers.push_back( box_marker );
00063
00064
00065 int_marker.controls.push_back( box_control );
00066
00067
00068
00069
00070 visualization_msgs::InteractiveMarkerControl linear_control;
00071 linear_control.name = "move_x";
00072 linear_control.interaction_mode =
00073 visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00074
00075
00076 int_marker.controls.push_back(linear_control);
00077
00078 return int_marker;
00079 }
00080
00081 bool is_red = false;
00082
00083 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00084 {
00085 ROS_INFO_STREAM( feedback->marker_name << " is now at "
00086 << feedback->pose.position.x << ", " << feedback->pose.position.y
00087 << ", " << feedback->pose.position.z );
00088
00089 bool changed = false;
00090 visualization_msgs::InteractiveMarker int_marker;
00091
00092
00093
00094 if( feedback->pose.position.x < 0 && !is_red )
00095 {
00096 printf( "turning red.\n" );
00097 is_red = true;
00098 int_marker = makeMarker( 1, 0, 0 );
00099 changed = true;
00100 }
00101 if( feedback->pose.position.x >= 0 && is_red )
00102 {
00103 printf( "turning green.\n" );
00104 is_red = false;
00105 int_marker = makeMarker( 0, 1, 0 );
00106 changed = true;
00107 }
00108
00109 if( changed )
00110 {
00111 printf( "changed.\n" );
00112 int_marker.pose = feedback->pose;
00113 server->insert( int_marker );
00114 server->applyChanges();
00115 }
00116 }
00117
00118 visualization_msgs::InteractiveMarker makeCrazyMarker( bool linear )
00119 {
00120
00121 visualization_msgs::InteractiveMarker int_marker;
00122 int_marker.header.frame_id = "/base_link";
00123 int_marker.name = "crazy_marker";
00124 int_marker.description = "Unusual 1-DOF Control";
00125
00126
00127 visualization_msgs::Marker box_marker;
00128 box_marker.type = visualization_msgs::Marker::CUBE;
00129 box_marker.scale.x = 1;
00130 box_marker.scale.y = .3;
00131 box_marker.scale.z = .3;
00132 box_marker.color.r = .3;
00133 box_marker.color.g = .1;
00134 box_marker.color.b = 1;
00135 box_marker.color.a = 1.0;
00136 box_marker.pose.position.y = -1.0;
00137
00138
00139 visualization_msgs::InteractiveMarkerControl box_control;
00140 box_control.always_visible = true;
00141 box_control.markers.push_back( box_marker );
00142 box_control.name = "crazy";
00143 if( linear )
00144 {
00145 box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00146 }
00147 else
00148 {
00149 box_control.orientation.w = 1;
00150 box_control.orientation.x = 0;
00151 box_control.orientation.y = 1;
00152 box_control.orientation.z = 0;
00153 box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00154 }
00155
00156
00157 int_marker.controls.push_back( box_control );
00158 int_marker.pose.position.y = 3;
00159
00160 return int_marker;
00161 }
00162
00163 bool is_linear = true;
00164
00165 void processCrazyFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00166 {
00167 ROS_INFO_STREAM( feedback->marker_name << " is now at pos "
00168 << feedback->pose.position.x << ", "
00169 << feedback->pose.position.y << ", "
00170 << feedback->pose.position.z << "; quat "
00171 << feedback->pose.orientation.x << ", "
00172 << feedback->pose.orientation.y << ", "
00173 << feedback->pose.orientation.z << ", "
00174 << feedback->pose.orientation.w );
00175
00176 bool changed = false;
00177 visualization_msgs::InteractiveMarker int_marker;
00178
00179
00180
00181 if( feedback->pose.orientation.z < 0 && !is_linear )
00182 {
00183 printf( "turning linear.\n" );
00184 is_linear = true;
00185 int_marker = makeCrazyMarker( true );
00186 changed = true;
00187 }
00188 if( feedback->pose.position.x > 0 && is_linear )
00189 {
00190 printf( "turning rotary.\n" );
00191 is_linear = false;
00192 int_marker = makeCrazyMarker( false );
00193 changed = true;
00194 }
00195
00196 if( changed )
00197 {
00198 printf( "changed.\n" );
00199 int_marker.pose.position.x = 0;
00200 int_marker.pose.orientation.x = 0;
00201 int_marker.pose.orientation.y = 0;
00202 int_marker.pose.orientation.z = 0;
00203 int_marker.pose.orientation.w = 1;
00204 server->insert( int_marker );
00205 server->applyChanges();
00206 }
00207 }
00208
00209 geometry_msgs::Pose pose;
00210
00211 visualization_msgs::InteractiveMarker make6DofMarker( bool fixed )
00212 {
00213 visualization_msgs::InteractiveMarker int_marker;
00214 int_marker.header.frame_id = "/base_link";
00215 int_marker.pose = pose;
00216 int_marker.scale = 1;
00217
00218 int_marker.name = "simple_6dof";
00219 int_marker.description = "Simple 6-DOF Control";
00220
00221
00222 visualization_msgs::Marker marker;
00223 marker.type = visualization_msgs::Marker::CUBE;
00224 marker.scale.x = .45;
00225 marker.scale.y = .45;
00226 marker.scale.z = .45;
00227 marker.color.r = 0.5;
00228 marker.color.g = 0.5;
00229 marker.color.b = 0.5;
00230 marker.color.a = 1.0;
00231 visualization_msgs::InteractiveMarkerControl control;
00232 control.always_visible = true;
00233 control.markers.push_back( marker );
00234 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
00235 control.description="Options";
00236 control.name = "menu_control";
00237 int_marker.controls.push_back( control );
00238
00239 control.markers.clear();
00240
00241 if ( fixed )
00242 {
00243 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00244 }
00245
00246 control.orientation.w = 1;
00247 control.orientation.x = 1;
00248 control.orientation.y = 0;
00249 control.orientation.z = 0;
00250 control.name = "rotate_x";
00251 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00252 int_marker.controls.push_back(control);
00253 control.name = "move_x";
00254 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00255 int_marker.controls.push_back(control);
00256
00257 control.orientation.w = 1;
00258 control.orientation.x = 0;
00259 control.orientation.y = 1;
00260 control.orientation.z = 0;
00261 control.name = "rotate_z";
00262 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00263 int_marker.controls.push_back(control);
00264 control.name = "move_z";
00265 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00266 int_marker.controls.push_back(control);
00267
00268 control.orientation.w = 1;
00269 control.orientation.x = 0;
00270 control.orientation.y = 0;
00271 control.orientation.z = 1;
00272 control.name = "rotate_y";
00273 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00274 int_marker.controls.push_back(control);
00275 control.name = "move_y";
00276 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00277 int_marker.controls.push_back(control);
00278
00279 return int_marker;
00280 }
00281
00282 interactive_markers::MenuHandler menu_handler;
00283
00284 void processFixedOrRelFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00285 {
00286 std::ostringstream s;
00287 s << "Feedback from marker '" << feedback->marker_name << "' "
00288 << " / control '" << feedback->control_name << "'";
00289
00290 std::ostringstream mouse_point_ss;
00291 if( feedback->mouse_point_valid )
00292 {
00293 mouse_point_ss << " at " << feedback->mouse_point.x
00294 << ", " << feedback->mouse_point.y
00295 << ", " << feedback->mouse_point.z
00296 << " in frame " << feedback->header.frame_id;
00297 }
00298
00299 switch ( feedback->event_type )
00300 {
00301 case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
00302 ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
00303 break;
00304
00305 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
00306 ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
00307 {
00308 visualization_msgs::InteractiveMarker fixed_or_rel_marker = make6DofMarker( feedback->menu_entry_id == 1 );
00309 server->insert(fixed_or_rel_marker, &processFixedOrRelFeedback);
00310 menu_handler.apply( *server, fixed_or_rel_marker.name );
00311 }
00312 break;
00313
00314 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00315 ROS_INFO_STREAM( s.str() << ": pose changed"
00316 << "\nposition = "
00317 << feedback->pose.position.x
00318 << ", " << feedback->pose.position.y
00319 << ", " << feedback->pose.position.z
00320 << "\norientation = "
00321 << feedback->pose.orientation.w
00322 << ", " << feedback->pose.orientation.x
00323 << ", " << feedback->pose.orientation.y
00324 << ", " << feedback->pose.orientation.z
00325 << "\nframe: " << feedback->header.frame_id
00326 << " time: " << feedback->header.stamp.sec << "sec, "
00327 << feedback->header.stamp.nsec << " nsec" );
00328 pose = feedback->pose;
00329 break;
00330
00331 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00332 ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
00333 break;
00334
00335 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00336 ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
00337 break;
00338 }
00339
00340 server->applyChanges();
00341 }
00342
00343 int main(int argc, char** argv)
00344 {
00345 ros::init(argc, argv, "interactive_marker_test");
00346
00347
00348 server = new interactive_markers::InteractiveMarkerServer("simple_marker");
00349
00350
00351 visualization_msgs::InteractiveMarker int_marker = makeMarker(0, 1, 0);
00352
00353
00354
00355 server->insert(int_marker, &processFeedback);
00356
00357
00358 visualization_msgs::InteractiveMarker crazy_marker = makeCrazyMarker( true );
00359
00360
00361
00362 server->insert(crazy_marker, &processCrazyFeedback);
00363
00364
00365
00366 pose.position.y = -3;
00367 pose.orientation.w = 1;
00368 visualization_msgs::InteractiveMarker fixed_or_rel_marker = make6DofMarker( true );
00369 server->insert(fixed_or_rel_marker, &processFixedOrRelFeedback);
00370 menu_handler.insert( "Fixed", &processFixedOrRelFeedback );
00371 menu_handler.insert( "Relative", &processFixedOrRelFeedback );
00372 menu_handler.apply( *server, fixed_or_rel_marker.name );
00373
00374
00375 server->applyChanges();
00376
00377
00378 ros::spin();
00379 }
00380