$search
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 // author: Adam Leeper 00031 00032 #include <interactive_marker_helpers/interactive_marker_helpers.h> 00033 00034 #include <tf/tf.h> 00035 #include <object_manipulator/tools/msg_helpers.h> 00036 00037 namespace im_helpers { 00038 00039 visualization_msgs::InteractiveMarker makeEmptyMarker( const char *frame_id ) 00040 { 00041 visualization_msgs::InteractiveMarker int_marker; 00042 int_marker.header.frame_id = frame_id; 00043 int_marker.scale = 1; 00044 00045 return int_marker; 00046 } 00047 00048 visualization_msgs::Marker makeBox( float scale ) 00049 { 00050 visualization_msgs::Marker marker; 00051 00052 marker.type = visualization_msgs::Marker::CUBE; 00053 marker.scale.x = scale; 00054 marker.scale.y = scale; 00055 marker.scale.z = scale; 00056 marker.color.r = 1.0; 00057 marker.color.g = 1.0; 00058 marker.color.b = 1.0; 00059 marker.color.a = 1.0; 00060 00061 return marker; 00062 } 00063 00064 visualization_msgs::Marker makeSphere( float scale ) 00065 { 00066 visualization_msgs::Marker marker; 00067 00068 marker.type = visualization_msgs::Marker::SPHERE; 00069 marker.scale.x = scale; 00070 marker.scale.y = scale; 00071 marker.scale.z = scale; 00072 marker.color.r = 1.0; 00073 marker.color.g = 1.0; 00074 marker.color.b = 1.0; 00075 marker.color.a = 1.0; 00076 00077 return marker; 00078 } 00079 00080 void add6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed ) 00081 { 00082 visualization_msgs::InteractiveMarkerControl control; 00083 00084 if(fixed) 00085 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; 00086 00087 control.orientation.w = 1; 00088 control.orientation.x = 1; 00089 control.orientation.y = 0; 00090 control.orientation.z = 0; 00091 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 00092 msg.controls.push_back(control); 00093 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 00094 msg.controls.push_back(control); 00095 00096 control.orientation.w = 1; 00097 control.orientation.x = 0; 00098 control.orientation.y = 1; 00099 control.orientation.z = 0; 00100 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 00101 msg.controls.push_back(control); 00102 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 00103 msg.controls.push_back(control); 00104 00105 control.orientation.w = 1; 00106 control.orientation.x = 0; 00107 control.orientation.y = 0; 00108 control.orientation.z = 1; 00109 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 00110 msg.controls.push_back(control); 00111 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 00112 msg.controls.push_back(control); 00113 } 00114 00115 00116 visualization_msgs::InteractiveMarkerControl& makeBoxControl( visualization_msgs::InteractiveMarker &msg ) 00117 { 00118 visualization_msgs::InteractiveMarkerControl control; 00119 control.always_visible = true; 00120 control.markers.push_back( makeBox(msg.scale) ); 00121 msg.controls.push_back( control ); 00122 00123 return msg.controls.back(); 00124 } 00125 00126 visualization_msgs::InteractiveMarkerControl& makeSphereControl( visualization_msgs::InteractiveMarker &msg ) 00127 { 00128 visualization_msgs::InteractiveMarkerControl control; 00129 control.always_visible = true; 00130 control.markers.push_back( makeSphere(msg.scale) ); 00131 msg.controls.push_back( control ); 00132 00133 return msg.controls.back(); 00134 } 00135 00136 visualization_msgs::MenuEntry makeMenuEntry(const char *title) 00137 { 00138 visualization_msgs::MenuEntry m; 00139 m.title = title; 00140 m.command = title; 00141 return m; 00142 } 00143 00144 visualization_msgs::MenuEntry makeMenuEntry(const char *title, const char *command, int type ) 00145 { 00146 visualization_msgs::MenuEntry m; 00147 m.title = title; 00148 m.command = command; 00149 m.command_type = type; 00150 return m; 00151 } 00152 00153 visualization_msgs::InteractiveMarker makePostureMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 00154 float scale, bool fixed, bool view_facing ) 00155 { 00156 visualization_msgs::InteractiveMarker int_marker; 00157 int_marker.header = stamped.header; 00158 int_marker.name = name; 00159 int_marker.scale = scale; 00160 int_marker.pose = stamped.pose; 00161 00162 visualization_msgs::InteractiveMarkerControl control; 00163 control.orientation.w = 1; 00164 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 00165 int_marker.controls.push_back(control); 00166 00167 return int_marker; 00168 } 00169 00170 visualization_msgs::InteractiveMarker makeHeadGoalMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 00171 float scale) 00172 { 00173 visualization_msgs::InteractiveMarker int_marker; 00174 int_marker.header = stamped.header; 00175 int_marker.name = name; 00176 int_marker.scale = scale; 00177 int_marker.pose = stamped.pose; 00178 00179 visualization_msgs::InteractiveMarkerControl control; 00180 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; 00181 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; 00182 control.orientation.w = 1; 00183 control.markers.push_back( makeSphere(scale*0.7) ); 00184 int_marker.controls.push_back(control); 00185 control.markers.clear(); 00186 00187 add6DofControl(int_marker); 00188 00189 return int_marker; 00190 } 00191 00192 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource, 00193 const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color, bool use_color ) 00194 { 00195 visualization_msgs::InteractiveMarker int_marker; 00196 int_marker.header = stamped.header; 00197 int_marker.pose = stamped.pose; 00198 int_marker.name = name; 00199 int_marker.scale = scale; 00200 00201 visualization_msgs::Marker mesh; 00202 mesh.color = color; 00203 mesh.mesh_resource = mesh_resource; 00204 mesh.mesh_use_embedded_materials = !use_color; 00205 mesh.type = visualization_msgs::Marker::MESH_RESOURCE; 00206 mesh.scale.x = scale; 00207 mesh.scale.y = scale; 00208 mesh.scale.z = scale; 00209 00210 visualization_msgs::InteractiveMarkerControl control; 00211 control.markers.push_back( mesh ); 00212 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00213 int_marker.controls.push_back( control ); 00214 00215 return int_marker; 00216 } 00217 00218 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource, 00219 const geometry_msgs::PoseStamped &stamped, float scale) 00220 { 00221 std_msgs::ColorRGBA color; 00222 return makeMeshMarker( name, mesh_resource, stamped, scale, color, false); 00223 } 00224 00225 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource, 00226 const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color) 00227 { 00228 return makeMeshMarker( name, mesh_resource, stamped, scale, color, true); 00229 } 00230 00231 visualization_msgs::InteractiveMarker makeButtonBox( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing ) 00232 { 00233 visualization_msgs::InteractiveMarker int_marker; 00234 int_marker.header = stamped.header; 00235 int_marker.name = name; 00236 int_marker.scale = scale; 00237 int_marker.pose = stamped.pose; 00238 //int_marker.description = "This is the marker."; 00239 00240 visualization_msgs::InteractiveMarkerControl &control = makeBoxControl(int_marker); 00241 //control.description = "This is the control"; 00242 control.always_visible = false; 00243 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00244 00245 return int_marker; 00246 } 00247 00248 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped, 00249 float scale, bool fixed, bool view_facing) 00250 { 00251 std_msgs::ColorRGBA color; 00252 return makeButtonSphere(name, stamped, scale, fixed, view_facing, color); 00253 } 00254 00255 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped, 00256 float scale, bool fixed, bool view_facing, 00257 std_msgs::ColorRGBA color) 00258 { 00259 visualization_msgs::InteractiveMarker int_marker; 00260 int_marker.header = stamped.header; 00261 int_marker.name = name; 00262 int_marker.scale = scale; 00263 int_marker.pose = stamped.pose; 00264 //int_marker.description = "This is the marker."; 00265 00266 visualization_msgs::InteractiveMarkerControl &control = makeSphereControl(int_marker); 00267 //control.description = "This is the control"; 00268 control.always_visible = false; 00269 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00270 control.markers.back().color = color; 00271 00272 return int_marker; 00273 } 00274 00275 visualization_msgs::InteractiveMarker makeListControl( const char *name, const geometry_msgs::PoseStamped &stamped, int num, int total, float scale) 00276 { 00277 00278 visualization_msgs::InteractiveMarker int_marker; 00279 int_marker.header = stamped.header; 00280 int_marker.name = name; 00281 int_marker.scale = scale; 00282 int_marker.pose = stamped.pose; 00283 00284 visualization_msgs::InteractiveMarkerControl control; 00285 00286 // control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; 00287 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00288 std::stringstream ss; 00289 ss << "grasp(" << num << "/" << total << ")"; 00290 interactive_markers::makeViewFacingButton(int_marker, control, ss.str()); 00291 00292 // control.orientation.w = 1; 00293 // control.orientation.x = 0; 00294 // control.orientation.y = -1; 00295 // control.orientation.z = 0; 00296 00297 // interactive_markers::makeArrow( int_marker, control, 0.3*scale ); 00298 // control.markers.back().color.r = 1.0; 00299 // control.markers.back().color.g = 0.0; 00300 // control.markers.back().color.b = 0.0; 00301 // control.name = "right"; 00302 // int_marker.controls.push_back(control); 00303 // 00304 // control.markers.clear(); 00305 // interactive_markers::makeArrow( int_marker, control, -0.3*scale ); 00306 // control.markers.back().color.r = 0.0; 00307 // control.markers.back().color.g = 1.0; 00308 // control.markers.back().color.b = 0.0; 00309 // control.name = "left"; 00310 int_marker.controls.push_back(control); 00311 00312 return int_marker; 00313 00314 00315 // visualization_msgs::InteractiveMarker int_marker; 00316 // int_marker.header = stamped.header; 00317 // int_marker.name = name; 00318 // int_marker.scale = scale; 00319 // int_marker.pose = stamped.pose; 00320 // //int_marker.description = "This is the marker."; 00321 // 00322 // visualization_msgs::InteractiveMarkerControl &control = makeSphereControl(int_marker); 00323 // //control.description = "This is the control"; 00324 // control.always_visible = false; 00325 // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00326 // 00327 // return int_marker; 00328 } 00329 00330 visualization_msgs::InteractiveMarker make6DofMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing ) 00331 { 00332 visualization_msgs::InteractiveMarker int_marker; 00333 int_marker.header = stamped.header; 00334 int_marker.name = name; 00335 int_marker.scale = scale; 00336 int_marker.pose = stamped.pose; 00337 00338 if ( view_facing ) 00339 { 00340 visualization_msgs::InteractiveMarkerControl control; 00341 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; 00342 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE; 00343 control.orientation.w = 1; 00344 int_marker.controls.push_back(control); 00345 00346 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; 00347 control.markers.push_back( makeSphere(scale*0.5) ); 00348 int_marker.controls.push_back(control); 00349 } 00350 else 00351 { 00352 add6DofControl(int_marker, fixed); 00353 } 00354 00355 return int_marker; 00356 } 00357 00358 visualization_msgs::InteractiveMarker makePlanarMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed ) 00359 { 00360 visualization_msgs::InteractiveMarker int_marker; 00361 int_marker.header = stamped.header; 00362 int_marker.name = name; 00363 int_marker.scale = scale; 00364 int_marker.pose = stamped.pose; 00365 00366 visualization_msgs::InteractiveMarkerControl control; 00367 control.orientation_mode = fixed ? (uint8_t)visualization_msgs::InteractiveMarkerControl::FIXED : (uint8_t)visualization_msgs::InteractiveMarkerControl::INHERIT; 00368 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE; 00369 control.orientation.w = 1; 00370 control.orientation.x = 0; 00371 control.orientation.y = 1; 00372 control.orientation.z = 0; 00373 int_marker.controls.push_back(control); 00374 00375 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; 00376 interactive_markers::makeArrow( int_marker, control, 0 ); 00377 control.markers.back().pose.orientation.w = 0; 00378 control.markers.back().pose.orientation.x = 0; 00379 control.markers.back().pose.orientation.y = 1; 00380 control.markers.back().pose.orientation.z = 0; 00381 control.markers.back().color.r = 0.0; 00382 control.markers.back().color.g = 1.0; 00383 control.markers.back().color.b = 0.0; 00384 int_marker.controls.push_back(control); 00385 00386 return int_marker; 00387 } 00388 00389 visualization_msgs::InteractiveMarker makeElevatorMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed) 00390 { 00391 visualization_msgs::InteractiveMarker int_marker; 00392 int_marker.header = stamped.header; 00393 int_marker.name = name; 00394 int_marker.scale = scale; 00395 int_marker.pose = stamped.pose; 00396 00397 visualization_msgs::InteractiveMarkerControl control; 00398 00399 if(fixed) 00400 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; 00401 00402 control.orientation.w = 1; 00403 control.orientation.x = 0; 00404 control.orientation.y = -1; 00405 control.orientation.z = 0; 00406 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00407 00408 interactive_markers::makeArrow( int_marker, control, 0.3 ); 00409 control.markers.back().color.r = 0.0; 00410 control.markers.back().color.g = 1.0; 00411 control.markers.back().color.b = 0.0; 00412 control.name = "up"; 00413 int_marker.controls.push_back(control); 00414 00415 control.markers.clear(); 00416 interactive_markers::makeArrow( int_marker, control, -0.3 ); 00417 control.markers.back().color.r = 1.0; 00418 control.markers.back().color.g = 0.0; 00419 control.markers.back().color.b = 0.0; 00420 control.name = "down"; 00421 int_marker.controls.push_back(control); 00422 00423 return int_marker; 00424 } 00425 00426 visualization_msgs::InteractiveMarker makeProjectorMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale) 00427 { 00428 visualization_msgs::InteractiveMarker int_marker; 00429 int_marker.header = stamped.header; 00430 int_marker.name = name; 00431 int_marker.scale = scale; 00432 int_marker.pose = stamped.pose; 00433 00434 visualization_msgs::InteractiveMarkerControl control; 00435 control.orientation.w = 1; 00436 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00437 visualization_msgs::Marker marker; 00438 marker.type = visualization_msgs::Marker::CYLINDER; 00439 marker.scale.x = 0.03; 00440 marker.scale.y = 0.03; 00441 marker.scale.z = 0.04; 00442 marker.color.r = 1.0; 00443 marker.color.a = 0.8; 00444 control.markers.push_back(marker); 00445 int_marker.controls.push_back(control); 00446 00447 return int_marker; 00448 } 00449 00450 00451 visualization_msgs::InteractiveMarker makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed) 00452 { 00453 visualization_msgs::InteractiveMarker int_marker; 00454 int_marker.header = stamped.header; 00455 int_marker.name = name; 00456 int_marker.scale = scale; 00457 int_marker.pose = stamped.pose; 00458 00459 visualization_msgs::InteractiveMarkerControl control; 00460 00461 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 00462 control.orientation.w = 1; 00463 control.orientation.y = -1; 00464 int_marker.controls.push_back(control); 00465 00466 00467 if(fixed) 00468 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; 00469 00470 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00471 control.orientation.w = 1; 00472 control.orientation.y = 0; 00473 00474 control.markers.clear(); 00475 interactive_markers::makeArrow( int_marker, control, 0.9 ); 00476 control.markers.back().color.r = 1.0; 00477 control.markers.back().color.g = 0.0; 00478 control.markers.back().color.b = 0.0; 00479 control.name = "forward"; 00480 int_marker.controls.push_back(control); 00481 00482 control.markers.clear(); 00483 interactive_markers::makeArrow( int_marker, control, -0.9 ); 00484 control.markers.back().color.r = 1.0; 00485 control.markers.back().color.g = 0.0; 00486 control.markers.back().color.b = 0.0; 00487 control.name = "back"; 00488 int_marker.controls.push_back(control); 00489 00490 control.orientation.z = 1; 00491 control.markers.clear(); 00492 interactive_markers::makeArrow( int_marker, control, 0.9 ); 00493 control.markers.back().color.r = 1.0; 00494 control.markers.back().color.g = 0.0; 00495 control.markers.back().color.b = 0.0; 00496 control.name = "left"; 00497 int_marker.controls.push_back(control); 00498 00499 control.markers.clear(); 00500 interactive_markers::makeArrow( int_marker, control, -0.9 ); 00501 control.markers.back().color.r = 1.0; 00502 control.markers.back().color.g = 0.0; 00503 control.markers.back().color.b = 0.0; 00504 control.name = "right"; 00505 int_marker.controls.push_back(control); 00506 00507 control.markers.clear(); 00508 tf::quaternionTFToMsg( btQuaternion(btVector3(0,0,1), 135*M_PI/180.0), control.orientation); 00509 interactive_markers::makeArrow( int_marker, control, 1.0 ); 00510 control.markers.back().pose.position.x = 0.7; 00511 control.markers.back().color.r = 1.0; 00512 control.markers.back().color.g = 1.0; 00513 control.markers.back().color.b = 0.0; 00514 control.name = "rotate left"; 00515 int_marker.controls.push_back(control); 00516 00517 control.markers.clear(); 00518 tf::quaternionTFToMsg( btQuaternion(btVector3(0,0,1), -135*M_PI/180.0), control.orientation); 00519 interactive_markers::makeArrow( int_marker, control, 1.0 ); 00520 control.markers.back().pose.position.x = 0.7; 00521 control.markers.back().color.r = 1.0; 00522 control.markers.back().color.g = 1.0; 00523 control.markers.back().color.b = 0.0; 00524 control.name = "rotate right"; 00525 int_marker.controls.push_back(control); 00526 return int_marker; 00527 } 00528 00529 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 00530 float scale, float angle, bool view_facing) 00531 { 00532 std_msgs::ColorRGBA color; 00533 return makeGripperMarker( name, stamped, scale, angle, view_facing, color, false); 00534 } 00535 00536 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 00537 float scale, float angle, bool view_facing, std_msgs::ColorRGBA color ) 00538 { 00539 return makeGripperMarker( name, stamped, scale, angle, view_facing, color, true); 00540 } 00541 00542 00543 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 00544 float scale, float angle, bool view_facing, std_msgs::ColorRGBA color, bool use_color ) 00545 { 00546 visualization_msgs::InteractiveMarker int_marker; 00547 int_marker.header = stamped.header; 00548 int_marker.name = name; 00549 int_marker.scale = 1.0; //scale; 00550 int_marker.pose = stamped.pose; 00551 00552 // add6DofControl(int_marker, false); 00553 00554 visualization_msgs::InteractiveMarkerControl control; 00555 00556 visualization_msgs::Marker mesh; 00557 mesh.mesh_use_embedded_materials = !use_color; 00558 mesh.type = visualization_msgs::Marker::MESH_RESOURCE; 00559 mesh.scale.x = scale; 00560 mesh.scale.y = scale; 00561 mesh.scale.z = scale; 00562 mesh.color = color; 00563 00564 tf::Transform T1, T2; 00565 tf::Transform T_proximal, T_distal; 00566 00567 T1.setOrigin(tf::Vector3(0.07691, 0.01, 0)); 00568 T1.setRotation(tf::Quaternion(tf::Vector3(0,0,1), angle)); 00569 T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0)); 00570 T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle)); 00571 T_proximal = T1; 00572 T_distal = T1 * T2; 00573 00574 mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"; 00575 mesh.pose.orientation.w = 1; 00576 control.markers.push_back( mesh ); 00577 mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"; 00578 mesh.pose = object_manipulator::msg::createPoseMsg(T_proximal); 00579 control.markers.push_back( mesh ); 00580 mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"; 00581 mesh.pose = object_manipulator::msg::createPoseMsg(T_distal); 00582 control.markers.push_back( mesh ); 00583 00584 T1.setOrigin(tf::Vector3(0.07691, -0.01, 0)); 00585 T1.setRotation(tf::Quaternion(tf::Vector3(1,0,0), M_PI)*tf::Quaternion(tf::Vector3(0,0,1), angle)); 00586 T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0)); 00587 T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle)); 00588 T_proximal = T1; 00589 T_distal = T1 * T2; 00590 00591 mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"; 00592 mesh.pose = object_manipulator::msg::createPoseMsg(T_proximal); 00593 control.markers.push_back( mesh ); 00594 mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"; 00595 mesh.pose = object_manipulator::msg::createPoseMsg(T_distal); 00596 control.markers.push_back( mesh ); 00597 00598 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00599 int_marker.controls.push_back( control ); 00600 00601 return int_marker; 00602 } 00603 00604 00605 visualization_msgs::InteractiveMarker makeGraspMarker( const char * name, const geometry_msgs::PoseStamped &stamped, float scale, PoseState pose_state) 00606 { 00607 visualization_msgs::InteractiveMarker int_marker; 00608 int_marker.header = stamped.header; 00609 int_marker.header.stamp = ros::Time(0); 00610 int_marker.name = name; 00611 int_marker.scale = scale; 00612 int_marker.pose = stamped.pose; 00613 00614 visualization_msgs::InteractiveMarkerControl control; 00615 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; 00616 control.orientation.w = 1; 00617 visualization_msgs::Marker marker; 00618 marker.type = visualization_msgs::Marker::ARROW; 00619 //marker.header = int_marker.header; 00620 //marker.pose = int_marker.pose; 00621 marker.scale.x = 0.10; 00622 marker.scale.y = 0.05; 00623 marker.scale.z = 0.05; 00624 marker.color.r = 1.0; 00625 marker.color.a = 1.0; 00626 control.markers.push_back(marker); 00627 marker.type = visualization_msgs::Marker::CUBE; 00628 marker.scale.x = 0.01; 00629 marker.scale.y = 0.04; 00630 marker.scale.z = 0.01; 00631 switch (pose_state) 00632 { 00633 case UNTESTED: 00634 marker.color.g = 0.5; 00635 marker.color.b = 0.5; 00636 marker.color.r = 0.5; 00637 break; 00638 case VALID: 00639 marker.color.g = 1.0; 00640 marker.color.r = 0.0; 00641 break; 00642 case INVALID: 00643 marker.color.g = 0.0; 00644 marker.color.r = 1.0; 00645 break; 00646 } 00647 control.markers.push_back(marker); 00648 // marker.points[0] = object_manipulator::msg::createPointMsg(0,0,0); 00649 // marker.points[1] = object_manipulator::msg::createPointMsg(0,0.03,0); 00650 // marker.color.r = 0; 00651 // marker.color.g = 1.0; 00652 // control.markers.push_back(marker); 00653 00654 int_marker.controls.push_back(control); 00655 00656 return int_marker; 00657 } 00658 00659 00660 visualization_msgs::InteractiveMarker makeRobotModelMarker( const char * name, const geometry_msgs::PoseStamped &stamped, 00661 const std::vector< geometry_msgs::PoseStamped> &mesh_poses, 00662 const std::vector<std::string> &mesh_paths, const float &scale, 00663 const bool robot_button_only) 00664 { 00665 //ROS_ERROR("Start create robot marker..."); 00666 visualization_msgs::InteractiveMarker int_marker; 00667 int_marker.header = stamped.header; 00668 int_marker.name = name; 00669 int_marker.scale = scale; 00670 int_marker.pose = stamped.pose; 00671 00672 bool fixed = false; 00673 visualization_msgs::InteractiveMarkerControl control; 00674 if( !robot_button_only ) 00675 { 00676 control.orientation_mode = fixed ? (uint8_t)visualization_msgs::InteractiveMarkerControl::FIXED : (uint8_t)visualization_msgs::InteractiveMarkerControl::INHERIT; 00677 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE; 00678 control.orientation.w = 1; 00679 control.orientation.x = 0; 00680 control.orientation.y = 1; 00681 control.orientation.z = 0; 00682 // //ROS_ERROR("Making disc..."); 00683 // interactive_markers::makeDisc(int_marker, control ); 00684 // //ROS_ERROR("Changing heights, size = %zd", control.markers.size()); 00685 // for(size_t i = 0; i < control.markers.size(); i++) 00686 // { 00687 // visualization_msgs::Marker m = control.markers[i]; 00688 // m.pose.position.z = 0.20; 00689 // } 00690 int_marker.controls.push_back(control); 00691 } 00692 00693 control.markers.clear(); 00694 control.interaction_mode = robot_button_only ? (visualization_msgs::InteractiveMarkerControl::NONE) : (visualization_msgs::InteractiveMarkerControl::MOVE_PLANE); 00695 for(size_t i = 0; i < mesh_poses.size(); i++) 00696 { 00697 visualization_msgs::Marker mesh; 00698 mesh.color = std_msgs::ColorRGBA(); 00699 mesh.mesh_resource = mesh_paths[i]; 00700 mesh.mesh_use_embedded_materials = true; 00701 mesh.type = visualization_msgs::Marker::MESH_RESOURCE; 00702 mesh.scale.x = mesh.scale.y = mesh.scale.z = scale; 00703 mesh.pose = mesh_poses[i].pose; 00704 control.markers.push_back( mesh ); 00705 } 00706 int_marker.controls.push_back( control ); 00707 00708 return int_marker; 00709 00710 } 00711 00712 } // namespace im_helpers