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