ROS学习笔记(七)::RVIZ::Interactive Markers: Basic Controls
1.Interactive Markers: Basic Controls基本控件
说明：这篇教程说明base_controls教程代码如何工作
2.basic_controls教程说明：
所有交互标志物包含一个灰色盒子。在大多数情况下，这个灰色盒子会与对照的其余控件一起移动。它会告诉你互动标志物的坐标系如何移动。
这显示了如何控件使用6个独立控件，控件6个自由度。使用环去旋转和箭头移动的结构。
4.Simple 6-DOF control (fixed orientation)简单的六自由度控件（固定方向）
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
48 Marker makeBox( InteractiveMarker &msg ) 49 { 50 Marker marker; 51 52 marker.type = Marker::CUBE; 53 marker.scale.x = msg.scale * 0.45; 54 marker.scale.y = msg.scale * 0.45; 55 marker.scale.z = msg.scale * 0.45; 56 marker.color.r = 0.5; 57 marker.color.g = 0.5; 58 marker.color.b = 0.5; 59 marker.color.a = 1.0; 60 61 return marker; 62 } 63 64 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) 65 { 66 InteractiveMarkerControl control; 67 control.always_visible = true; 68 control.markers.push_back( makeBox(msg) ); 69 msg.controls.push_back( control ); 70 71 return msg.controls.back(); 72 }
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
183 void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector3& position, bool show_6dof ) 184 { 185 InteractiveMarker int_marker; 186 int_marker.header.frame_id = "base_link"; 187 tf::pointTFToMsg(position, int_marker.pose.position); 188 int_marker.scale = 1; 189 190 int_marker.name = "simple_6dof"; 191 int_marker.description = "Simple 6-DOF Control"; 192 193 // insert a box 194 makeBoxControl(int_marker); 195 int_marker.controls[0].interaction_mode = interaction_mode; 196 197 InteractiveMarkerControl control; 198 199 if ( fixed ) 200 { 201 int_marker.name += "_fixed"; 202 int_marker.description += "\n(fixed orientation)"; 203 control.orientation_mode = InteractiveMarkerControl::FIXED; 204 } 205 206 if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE) 207 { 208 std::string mode_text; 209 if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_3D ) mode_text = "MOVE_3D"; 210 if( interaction_mode == visualization_msgs::InteractiveMarkerControl::ROTATE_3D ) mode_text = "ROTATE_3D"; 211 if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D ) mode_text = "MOVE_ROTATE_3D"; 212 int_marker.name += "_" + mode_text; 213 int_marker.description = std::string("3D Control") + (show_6dof ? " + 6-DOF controls" : "") + "\n" + mode_text; 214 } 215 216 if(show_6dof) 217 { 218 control.orientation.w = 1; 219 control.orientation.x = 1; 220 control.orientation.y = 0; 221 control.orientation.z = 0; 222 control.name = "rotate_x"; 223 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 224 int_marker.controls.push_back(control); 225 control.name = "move_x"; 226 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 227 int_marker.controls.push_back(control); 228 229 control.orientation.w = 1; 230 control.orientation.x = 0; 231 control.orientation.y = 1; 232 control.orientation.z = 0; 233 control.name = "rotate_z"; 234 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 235 int_marker.controls.push_back(control); 236 control.name = "move_z"; 237 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 238 int_marker.controls.push_back(control); 239 240 control.orientation.w = 1; 241 control.orientation.x = 0; 242 control.orientation.y = 0; 243 control.orientation.z = 1; 244 control.name = "rotate_y"; 245 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 246 int_marker.controls.push_back(control); 247 control.name = "move_y"; 248 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 249 int_marker.controls.push_back(control); 250 } 251 252 server->insert(int_marker); 253 server->setCallback(int_marker.name, &processFeedback); 254 if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE) 255 menu_handler.apply( *server, int_marker.name ); 256 }
这些新的标记物类型用鼠标可以支持各类3D动作。 *MOVE_3D：在本教程中绘制成 箱标记物，这种互动模式允许标记物的3D转换（在相机平面默认情况下，进/出摄像头，同时按住shift。（亲测，只能使单独一个放大缩小））。 ROTATE_3D：在本教程中绘制成箱标记物，这种interacton模式允许标记物的3D旋转（同时按住Shift键时候，默认情况下，可以绕着相机水平面的垂直和水平轴，并绕轴垂直于相机水平面，）。 MOVE_ROTATE_3D：这种互动方式是MOVE_3D（默认）和ROTATE_3D联盟（同时按住Ctrl键）。一个互动的标志物可以有多个冗余的控件方式;在本教程中，盒子是一个3D控件，然而标记物也有一组简单的6自由度的环和箭头组合。可以写一个Rviz插件，这个插件允许这些标记物使用6维输入装置进行三维抓取。这些六维输入装置如Phantom Omni或者Razer Hydra。见http://www.ros.org/wiki/interaction_cursor_rviz。6.6-DOF (Arbitrary Axes) 6自由度（任意轴）
258 void makeRandomDofMarker( const tf::Vector3& position )
259 {
260 InteractiveMarker int_marker;
261 int_marker.header.frame_id = "base_link";
262 tf::pointTFToMsg(position, int_marker.pose.position);
263 int_marker.scale = 1;
264
265 int_marker.name = "6dof_random_axes";
266 int_marker.description = "6-DOF\n(Arbitrary Axes)";
267
268 makeBoxControl(int_marker);
269
270 InteractiveMarkerControl control;
271
272 for ( int i=0; i<3; i++ )
273 {
274 control.orientation.w = rand(-1,1);
275 control.orientation.x = rand(-1,1);
276 control.orientation.y = rand(-1,1);
277 control.orientation.z = rand(-1,1);
278 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
279 int_marker.controls.push_back(control);
280 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
281 int_marker.controls.push_back(control);
282 }
283
284 server->insert(int_marker);
285 server->setCallback(int_marker.name, &processFeedback);
286 }
通过给能够确定每个控件的定向四元数分配随机值来创建在本实施例的控件。 RViz使这些四元数正常化，所以你在创建一个互动标记物时不必担心。
7.View-Facing 6-DOF
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
289 void makeViewFacingMarker( const tf::Vector3& position ) 290 { 291 InteractiveMarker int_marker; 292 int_marker.header.frame_id = "base_link"; 293 tf::pointTFToMsg(position, int_marker.pose.position); 294 int_marker.scale = 1; 295 296 int_marker.name = "view_facing"; 297 int_marker.description = "View Facing 6-DOF"; 298 299 InteractiveMarkerControl control; 300 301 // make a control that rotates around the view axis 302 control.orientation_mode = InteractiveMarkerControl::VIEW_FACING; 303 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 304 control.orientation.w = 1; 305 control.name = "rotate"; 306 307 int_marker.controls.push_back(control); 308 309 // create a box in the center which should not be view facing, 310 // but move in the camera plane. 311 control.orientation_mode = InteractiveMarkerControl::VIEW_FACING; 312 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; 313 control.independent_marker_orientation = true; 314 control.name = "move"; 315 316 control.markers.push_back( makeBox(int_marker) ); 317 control.always_visible = true; 318 319 int_marker.controls.push_back(control); 320 321 server->insert(int_marker); 322 server->setCallback(int_marker.name, &processFeedback); 323 }
8.Quadrocopter四轴飞行器
点击并拖动绿环，看看如何结合运动和旋转的工作原理：如果鼠标光标停留靠近环，它只会旋转。一旦你将它渐行渐远，它会开始跟随鼠标。
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
326 void makeQuadrocopterMarker( const tf::Vector3& position ) 327 { 328 InteractiveMarker int_marker; 329 int_marker.header.frame_id = "base_link"; 330 tf::pointTFToMsg(position, int_marker.pose.position); 331 int_marker.scale = 1; 332 333 int_marker.name = "quadrocopter"; 334 int_marker.description = "Quadrocopter"; 335 336 makeBoxControl(int_marker); 337 338 InteractiveMarkerControl control; 339 340 control.orientation.w = 1; 341 control.orientation.x = 0; 342 control.orientation.y = 1; 343 control.orientation.z = 0; 344 control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE; 345 int_marker.controls.push_back(control); 346 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 347 int_marker.controls.push_back(control); 348 349 server->insert(int_marker); 350 server->setCallback(int_marker.name, &processFeedback); 351 }
9.Chess Piece国际象棋棋子
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
353 void makeChessPieceMarker( const tf::Vector3& position ) 354 { 355 InteractiveMarker int_marker; 356 int_marker.header.frame_id = "base_link"; 357 tf::pointTFToMsg(position, int_marker.pose.position); 358 int_marker.scale = 1; 359 360 int_marker.name = "chess_piece"; 361 int_marker.description = "Chess Piece\n(2D Move + Alignment)"; 362 363 InteractiveMarkerControl control; 364 365 control.orientation.w = 1; 366 control.orientation.x = 0; 367 control.orientation.y = 1; 368 control.orientation.z = 0; 369 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; 370 int_marker.controls.push_back(control); 371 372 // make a box which also moves in the plane 373 control.markers.push_back( makeBox(int_marker) ); 374 control.always_visible = true; 375 int_marker.controls.push_back(control); 376 377 // we want to use our special callback function 378 server->insert(int_marker); 379 server->setCallback(int_marker.name, &processFeedback); 380 381 // set different callback for POSE_UPDATE feedback 382 server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE ); 383 }
The major difference to the previous example is that an additional feedback function is specified, which will be called instead of processFeedback() when the pose of the marker gets updated. This function modifies the pose of the marker and sends it back to RViz:
148 void alignMarker( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 149 { 150 geometry_msgs::Pose pose = feedback->pose; 151 152 pose.position.x = round(pose.position.x-0.5)+0.5; 153 pose.position.y = round(pose.position.y-0.5)+0.5; 154 155 ROS_INFO_STREAM( feedback->marker_name << ":" 156 << " aligning position = " 157 << feedback->pose.position.x 158 << ", " << feedback->pose.position.y 159 << ", " << feedback->pose.position.z 160 << " to " 161 << pose.position.x 162 << ", " << pose.position.y 163 << ", " << pose.position.z ); 164 165 server->setPose( feedback->marker_name, pose ); 166 server->applyChanges(); 167 }
10.Pan / Tilt平移/倾斜
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
385 void makePanTiltMarker( const tf::Vector3& position ) 386 { 387 InteractiveMarker int_marker; 388 int_marker.header.frame_id = "base_link"; 389 tf::pointTFToMsg(position, int_marker.pose.position); 390 int_marker.scale = 1; 391 392 int_marker.name = "pan_tilt"; 393 int_marker.description = "Pan / Tilt"; 394 395 makeBoxControl(int_marker); 396 397 InteractiveMarkerControl control; 398 399 control.orientation.w = 1; 400 control.orientation.x = 0; 401 control.orientation.y = 1; 402 control.orientation.z = 0; 403 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 404 control.orientation_mode = InteractiveMarkerControl::FIXED; 405 int_marker.controls.push_back(control); 406 407 control.orientation.w = 1; 408 control.orientation.x = 0; 409 control.orientation.y = 0; 410 control.orientation.z = 1; 411 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 412 control.orientation_mode = InteractiveMarkerControl::INHERIT; 413 int_marker.controls.push_back(control); 414 415 server->insert(int_marker); 416 server->setCallback(int_marker.name, &processFeedback); 417 }
11Context Menu上下文菜单（不知道啥意思）
这个例子显示了如何将一个简单的静态菜单附加到一个交互标记物。如果没有指定自定义标记的可视化（如灰色框的情况下），RViz会创建一个文本标记物浮在交互标记物的上面，你能打开上下文菜单。
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
419 void makeMenuMarker( const tf::Vector3& position ) 420 { 421 InteractiveMarker int_marker; 422 int_marker.header.frame_id = "base_link"; 423 tf::pointTFToMsg(position, int_marker.pose.position); 424 int_marker.scale = 1; 425 426 int_marker.name = "context_menu"; 427 int_marker.description = "Context Menu\n(Right Click)"; 428 429 InteractiveMarkerControl control; 430 431 control.interaction_mode = InteractiveMarkerControl::MENU; 432 control.name = "menu_only_control"; 433 434 Marker marker = makeBox( int_marker ); 435 control.markers.push_back( marker ); 436 control.always_visible = true; 437 int_marker.controls.push_back(control); 438 439 server->insert(int_marker); 440 server->setCallback(int_marker.name, &processFeedback); 441 menu_handler.apply( *server, int_marker.name ); 442 }
12.Button按钮
Button控件的行为与上例中的菜单控件几乎完全一样。您可以使用此类型以告知用户左击是交互所需的模式。 RViz将使用不同的鼠标光标这种类型的控件（ROS Groovy和以上）。
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
444 void makeButtonMarker( const tf::Vector3& position ) 445 { 446 InteractiveMarker int_marker; 447 int_marker.header.frame_id = "base_link"; 448 tf::pointTFToMsg(position, int_marker.pose.position); 449 int_marker.scale = 1; 450 451 int_marker.name = "button"; 452 int_marker.description = "Button\n(Left Click)"; 453 454 InteractiveMarkerControl control; 455 456 control.interaction_mode = InteractiveMarkerControl::BUTTON; 457 control.name = "button_control"; 458 459 Marker marker = makeBox( int_marker ); 460 control.markers.push_back( marker ); 461 control.always_visible = true; 462 int_marker.controls.push_back(control); 463 464 server->insert(int_marker); 465 server->setCallback(int_marker.name, &processFeedback); 466 }
13.Marker attached to a moving frame标记附着于活动坐标系
这个例子显示了如果点击一个附加框架的标记物，这个框架随着RViz指定的固定坐标系移动，将会发生什么。点击盒子移动和点击环旋转。对于含框架运动，标记物将继续相对你的鼠标移动，即使你不动它。在交互标记物排头的标记，必须ROS::Time（0）（如果未设置,它在默认情况下是这个，），让rviz将采取最新的TF坐标系来转换它。（最后一句没看懂）
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
468 void makeMovingMarker( const tf::Vector3& position ) 469 { 470 InteractiveMarker int_marker; 471 int_marker.header.frame_id = "moving_frame"; 472 tf::pointTFToMsg(position, int_marker.pose.position); 473 int_marker.scale = 1; 474 475 int_marker.name = "moving"; 476 int_marker.description = "Marker Attached to a\nMoving Frame"; 477 478 InteractiveMarkerControl control; 479 480 control.orientation.w = 1; 481 control.orientation.x = 1; 482 control.orientation.y = 0; 483 control.orientation.z = 0; 484 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 485 int_marker.controls.push_back(control); 486 487 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; 488 control.always_visible = true; 489 control.markers.push_back( makeBox(int_marker) ); 490 int_marker.controls.push_back(control); 491 492 server->insert(int_marker); 493 server->setCallback(int_marker.name, &processFeedback); 494 }
14.The surrounding code周围的代码
要设置服务器节点，所有需要的是创造InteractiveMarkerServer的一个实例，并发送所有InteractiveMarker信息到该对象上。
请注意，在添加，更新或删除交互标记物，他们的姿态，菜单或者反馈函数之后，你必须调用applyChanges（）。这将引起InteractiveMarkerServer将所有的sheduled changes应用到其内部状态，并且给所有连接的客户端发送一个更新的消息。这样做是确保在服务器和客户端保持同步状态，并尽量减少在服务器和客户端之间的数据通信。
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
44 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server; 45 interactive_markers::MenuHandler menu_handler;
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
496 int main(int argc, char** argv) 497 { 498 ros::init(argc, argv, "basic_controls"); 499 ros::NodeHandle n; 500 501 // create a timer to update the published transforms 502 ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback); 503 504 server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) ); 505 506 ros::Duration(0.1).sleep(); 507 508 menu_handler.insert( "First Entry", &processFeedback ); 509 menu_handler.insert( "Second Entry", &processFeedback ); 510 interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" ); 511 menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback ); 512 menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback ); 513 514 tf::Vector3 position; 515 position = tf::Vector3(-3, 3, 0); 516 make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::NONE, position, true ); 517 position = tf::Vector3( 0, 3, 0); 518 make6DofMarker( true, visualization_msgs::InteractiveMarkerControl::NONE, position, true ); 519 position = tf::Vector3( 3, 3, 0); 520 makeRandomDofMarker( position ); 521 position = tf::Vector3(-3, 0, 0); 522 make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::ROTATE_3D, position, false ); 523 position = tf::Vector3( 0, 0, 0); 524 make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D, position, true ); 525 position = tf::Vector3( 3, 0, 0); 526 make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_3D, position, false ); 527 position = tf::Vector3(-3,-3, 0); 528 makeViewFacingMarker( position ); 529 position = tf::Vector3( 0,-3, 0); 530 makeQuadrocopterMarker( position ); 531 position = tf::Vector3( 3,-3, 0); 532 makeChessPieceMarker( position ); 533 position = tf::Vector3(-3,-6, 0); 534 makePanTiltMarker( position ); 535 position = tf::Vector3( 0,-6, 0); 536 makeMovingMarker( position ); 537 position = tf::Vector3( 3,-6, 0); 538 makeMenuMarker( position ); 539 position = tf::Vector3( 0,-9, 0); 540 makeButtonMarker( position ); 541 542 server->applyChanges(); 543 544 ros::spin(); 545 546 server.reset(); 547 }
将定时器设置为更新base_link和moving_frame之间的TF转换，这是在frameCallback（）完成的：
https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
74 void frameCallback(const ros::TimerEvent&) 75 { 76 static uint32_t counter = 0; 77 78 static tf::TransformBroadcaster br; 79 80 tf::Transform t; 81 82 ros::Time time = ros::Time::now(); 83 84 t.setOrigin(tf::Vector3(0.0, 0.0, sin(float(counter)/140.0) * 2.0)); 85 t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0)); 86 br.sendTransform(tf::StampedTransform(t, time, "base_link", "moving_frame")); 87 88 t.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); 89 t.setRotation(tf::createQuaternionFromRPY(0.0, float(counter)/140.0, 0.0)); 90 br.sendTransform(tf::StampedTransform(t, time, "base_link", "rotating_frame")); 91 92 counter++; 93 }
最后，processFeedback（）被用于反馈到达时打印输出到rosconsole：https://raw.github.com/ros-visualization/visualization_tutorials/groovy-devel/interactive_marker_tutorials/src/basic_controls.cpp
95 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 96 { 97 std::ostringstream s; 98 s << "Feedback from marker '" << feedback->marker_name << "' " 99 << " / control '" << feedback->control_name << "'"; 100 101 std::ostringstream mouse_point_ss; 102 if( feedback->mouse_point_valid ) 103 { 104 mouse_point_ss << " at " << feedback->mouse_point.x 105 << ", " << feedback->mouse_point.y 106 << ", " << feedback->mouse_point.z 107 << " in frame " << feedback->header.frame_id; 108 } 109 110 switch ( feedback->event_type ) 111 { 112 case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: 113 ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." ); 114 break; 115 116 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: 117 ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." ); 118 break; 119 120 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: 121 ROS_INFO_STREAM( s.str() << ": pose changed" 122 << "\nposition = " 123 << feedback->pose.position.x 124 << ", " << feedback->pose.position.y 125 << ", " << feedback->pose.position.z 126 << "\norientation = " 127 << feedback->pose.orientation.w 128 << ", " << feedback->pose.orientation.x 129 << ", " << feedback->pose.orientation.y 130 << ", " << feedback->pose.orientation.z 131 << "\nframe: " << feedback->header.frame_id 132 << " time: " << feedback->header.stamp.sec << "sec, " 133 << feedback->header.stamp.nsec << " nsec" ); 134 break; 135 136 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: 137 ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." ); 138 break; 139 140 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: 141 ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." ); 142 break; 143 } 144 145 server->applyChanges(); 146 }