point_cloud_server: point_cloud_server: marker

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 "marker_helpers.h"
00033 
00034 #include <tf/tf.h>
00035 #include <object_manipulator/tools/msg_helpers.h>
00036 
00037 visualization_msgs::InteractiveMarker makeEmptyMarker( const char *frame_id )
00038 {
00039   visualization_msgs::InteractiveMarker int_marker;
00040   int_marker.header.frame_id = frame_id;
00041   int_marker.scale = 1;
00042 
00043   return int_marker;
00044 }
00045 
00046 visualization_msgs::Marker makeBox( visualization_msgs::InteractiveMarker &msg )
00047 {
00048   visualization_msgs::Marker marker;
00049 
00050   marker.type = visualization_msgs::Marker::CUBE;
00051   marker.scale.x = msg.scale * 0.45;
00052   marker.scale.y = msg.scale * 0.45;
00053   marker.scale.z = msg.scale * 0.45;
00054   marker.color.r = 1.0;
00055   marker.color.g = 1.0;
00056   marker.color.b = 1.0;
00057   marker.color.a = 1.0;
00058 
00059   return marker;
00060 }
00061 
00062 void add6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed )
00063 {
00064   visualization_msgs::InteractiveMarkerControl control;
00065 
00066   if(fixed)
00067     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00068 
00069   control.orientation.w = 1;
00070   control.orientation.x = 1;
00071   control.orientation.y = 0;
00072   control.orientation.z = 0;
00073   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00074   msg.controls.push_back(control);
00075   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00076   msg.controls.push_back(control);
00077 
00078   control.orientation.w = 1;
00079   control.orientation.x = 0;
00080   control.orientation.y = 1;
00081   control.orientation.z = 0;
00082   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00083   msg.controls.push_back(control);
00084   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00085   msg.controls.push_back(control);
00086 
00087   control.orientation.w = 1;
00088   control.orientation.x = 0;
00089   control.orientation.y = 0;
00090   control.orientation.z = 1;
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 
00097 visualization_msgs::Marker makeSphere( visualization_msgs::InteractiveMarker &msg, double scale )
00098 {
00099   visualization_msgs::Marker marker;
00100 
00101   marker.type = visualization_msgs::Marker::SPHERE;
00102   marker.scale.x = scale;
00103   marker.scale.y = scale;
00104   marker.scale.z = scale;
00105   marker.color.r = 1.0;
00106   marker.color.g = 1.0;
00107   marker.color.b = 1.0;
00108   marker.color.a = 1.0;
00109 
00110   return marker;
00111 }
00112 
00113 
00114 visualization_msgs::InteractiveMarkerControl& makeBoxControl( visualization_msgs::InteractiveMarker &msg )
00115 {
00116   visualization_msgs::InteractiveMarkerControl control;
00117   control.always_visible = true;
00118   control.markers.push_back( makeBox(msg) );
00119   msg.controls.push_back( control );
00120 
00121   return msg.controls.back();
00122 }
00123 
00124 visualization_msgs::MenuEntry makeMenuEntry(const char *title)
00125 {
00126   visualization_msgs::MenuEntry m;
00127   m.title = title;
00128   m.command = title;
00129   return m;
00130 }
00131 
00132 visualization_msgs::MenuEntry makeMenuEntry(const char *title, const char *command, int type  )
00133 {
00134   visualization_msgs::MenuEntry m;
00135   m.title = title;
00136   m.command = command;
00137   m.command_type = type;
00138   return m;
00139 }
00140 
00141 visualization_msgs::InteractiveMarker makePostureMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00142                                                          float scale, bool fixed, bool view_facing )
00143 {
00144   visualization_msgs::InteractiveMarker int_marker;
00145   int_marker.header =  stamped.header;
00146   int_marker.name = name;
00147   int_marker.scale = scale;
00148   int_marker.pose = stamped.pose;
00149 
00150   visualization_msgs::InteractiveMarkerControl control;
00151   control.orientation.w = 1;
00152   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00153   int_marker.controls.push_back(control);
00154 
00155   return int_marker;
00156 }
00157 
00158 visualization_msgs::InteractiveMarker makeHeadGoalMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00159                                                           float scale)
00160 {
00161   visualization_msgs::InteractiveMarker int_marker;
00162   int_marker.header =  stamped.header;
00163   int_marker.name = name;
00164   int_marker.scale = scale;
00165   int_marker.pose = stamped.pose;
00166 
00167   visualization_msgs::InteractiveMarkerControl control;
00168   control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
00169   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
00170   control.orientation.w = 1;
00171   control.markers.push_back( makeSphere(int_marker, scale*0.7) );
00172   int_marker.controls.push_back(control);
00173   control.markers.clear();
00174 
00175   add6DofControl(int_marker);
00176 
00177   return int_marker;
00178 }
00179 
00180 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00181                                                       const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color, bool use_color )
00182 {
00183   visualization_msgs::InteractiveMarker int_marker;
00184   int_marker.header =  stamped.header;
00185   int_marker.pose = stamped.pose;
00186   int_marker.name = name;
00187   int_marker.scale = scale;
00188 
00189   visualization_msgs::Marker mesh;
00190   mesh.color = color;
00191   mesh.mesh_resource = mesh_resource;
00192   mesh.mesh_use_embedded_materials = !use_color;
00193   mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
00194   mesh.scale.x = scale;
00195   mesh.scale.y = scale;
00196   mesh.scale.z = scale;
00197 
00198   visualization_msgs::InteractiveMarkerControl control;
00199   control.markers.push_back( mesh );
00200   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00201   int_marker.controls.push_back( control );
00202 
00203   return int_marker;
00204 }
00205 
00206 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00207                                                       const geometry_msgs::PoseStamped &stamped, float scale)
00208 {
00209   std_msgs::ColorRGBA color;
00210   return makeMeshMarker( name, mesh_resource, stamped, scale, color, false);
00211 }
00212 
00213 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00214                                                       const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color)
00215 {
00216   return makeMeshMarker( name, mesh_resource, stamped, scale, color, true);
00217 }
00218 
00219 visualization_msgs::InteractiveMarker makeButtonBox( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing )
00220 {
00221   visualization_msgs::InteractiveMarker int_marker;
00222   int_marker.header =  stamped.header;
00223   int_marker.name = name;
00224   int_marker.scale = scale;
00225   int_marker.pose = stamped.pose;
00226   //int_marker.description = "This is the marker.";
00227 
00228   visualization_msgs::InteractiveMarkerControl &control = makeBoxControl(int_marker);
00229   //control.description = "This is the control";
00230   control.always_visible = false;
00231   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00232 
00233   return int_marker;
00234 }
00235 
00236 visualization_msgs::InteractiveMarker make6DofMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing )
00237 {
00238   visualization_msgs::InteractiveMarker int_marker;
00239   int_marker.header =  stamped.header;
00240   int_marker.name = name;
00241   int_marker.scale = scale;
00242   int_marker.pose = stamped.pose;
00243 
00244   if ( view_facing )
00245   {
00246     visualization_msgs::InteractiveMarkerControl control;
00247     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
00248     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;
00249     control.orientation.w = 1;
00250     int_marker.controls.push_back(control);
00251 
00252     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
00253     control.markers.push_back( makeSphere(int_marker, scale*0.42) );
00254     int_marker.controls.push_back(control);
00255   }
00256   else
00257   {
00258     add6DofControl(int_marker, fixed);
00259   }
00260 
00261   return int_marker;
00262 }
00263 
00264 visualization_msgs::InteractiveMarker makeElevatorMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
00265 {
00266   visualization_msgs::InteractiveMarker int_marker;
00267   int_marker.header =  stamped.header;
00268   int_marker.name = name;
00269   int_marker.scale = scale;
00270   int_marker.pose = stamped.pose;
00271 
00272   visualization_msgs::InteractiveMarkerControl control;
00273 
00274   if(fixed)
00275     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00276 
00277   control.orientation.w = 1;
00278   control.orientation.x = 0;
00279   control.orientation.y = -1;
00280   control.orientation.z = 0;
00281   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00282 
00283   interactive_markers::makeArrow( int_marker, control, 0.3 );
00284   control.markers.back().color.r = 0.0;
00285   control.markers.back().color.g = 1.0;
00286   control.markers.back().color.b = 0.0;
00287   control.name = "up";
00288   int_marker.controls.push_back(control);
00289 
00290   control.markers.clear();
00291   interactive_markers::makeArrow( int_marker, control, -0.3 );
00292   control.markers.back().color.r = 1.0;
00293   control.markers.back().color.g = 0.0;
00294   control.markers.back().color.b = 0.0;
00295   control.name = "down";
00296   int_marker.controls.push_back(control);
00297 
00298   return int_marker;
00299 }
00300 
00301 visualization_msgs::InteractiveMarker makeProjectorMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale)
00302 {
00303   visualization_msgs::InteractiveMarker int_marker;
00304   int_marker.header =  stamped.header;
00305   int_marker.name = name;
00306   int_marker.scale = scale;
00307   int_marker.pose = stamped.pose;
00308 
00309   visualization_msgs::InteractiveMarkerControl control;
00310   control.orientation.w = 1;
00311   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00312   visualization_msgs::Marker marker;
00313   marker.type = visualization_msgs::Marker::CYLINDER;
00314   marker.scale.x = 0.03;
00315   marker.scale.y = 0.03;
00316   marker.scale.z = 0.04;
00317   marker.color.r = 1.0;
00318   marker.color.a = 0.8;
00319   control.markers.push_back(marker);
00320   int_marker.controls.push_back(control);
00321 
00322   return int_marker;
00323 }
00324 
00325 
00326 visualization_msgs::InteractiveMarker makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
00327 {
00328   visualization_msgs::InteractiveMarker int_marker;
00329   int_marker.header =  stamped.header;
00330   int_marker.name = name;
00331   int_marker.scale = scale;
00332   int_marker.pose = stamped.pose;
00333 
00334   visualization_msgs::InteractiveMarkerControl control;
00335 
00336   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00337   control.orientation.w = 1;
00338   control.orientation.y = -1;
00339   int_marker.controls.push_back(control);
00340 
00341 
00342   if(fixed)
00343     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00344 
00345   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00346   control.orientation.w = 1;
00347   control.orientation.y = 0;
00348 
00349   control.markers.clear();
00350   interactive_markers::makeArrow( int_marker, control, 0.9 );
00351   control.markers.back().color.r = 1.0;
00352   control.markers.back().color.g = 0.0;
00353   control.markers.back().color.b = 0.0;
00354   control.name = "forward";
00355   int_marker.controls.push_back(control);
00356 
00357   control.markers.clear();
00358   interactive_markers::makeArrow( int_marker, control, -0.9 );
00359   control.markers.back().color.r = 1.0;
00360   control.markers.back().color.g = 0.0;
00361   control.markers.back().color.b = 0.0;
00362   control.name = "back";
00363   int_marker.controls.push_back(control);
00364 
00365   control.orientation.z = 1;
00366   control.markers.clear();
00367   interactive_markers::makeArrow( int_marker, control, 0.9 );
00368   control.markers.back().color.r = 1.0;
00369   control.markers.back().color.g = 0.0;
00370   control.markers.back().color.b = 0.0;
00371   control.name = "left";
00372   int_marker.controls.push_back(control);
00373 
00374   control.markers.clear();
00375   interactive_markers::makeArrow( int_marker, control, -0.9 );
00376   control.markers.back().color.r = 1.0;
00377   control.markers.back().color.g = 0.0;
00378   control.markers.back().color.b = 0.0;
00379   control.name = "right";
00380   int_marker.controls.push_back(control);
00381 
00382   control.markers.clear();
00383   tf::quaternionTFToMsg( btQuaternion(btVector3(0,0,1), 135*M_PI/180.0), control.orientation);
00384   interactive_markers::makeArrow( int_marker, control, 1.0 );
00385   control.markers.back().pose.position.x = 0.7;
00386   control.markers.back().color.r = 1.0;
00387   control.markers.back().color.g = 1.0;
00388   control.markers.back().color.b = 0.0;
00389   control.name = "rotate left";
00390   int_marker.controls.push_back(control);
00391 
00392   control.markers.clear();
00393   tf::quaternionTFToMsg( btQuaternion(btVector3(0,0,1), -135*M_PI/180.0), control.orientation);
00394   interactive_markers::makeArrow( int_marker, control, 1.0 );
00395   control.markers.back().pose.position.x = 0.7;
00396   control.markers.back().color.r = 1.0;
00397   control.markers.back().color.g = 1.0;
00398   control.markers.back().color.b = 0.0;
00399   control.name = "rotate right";
00400   int_marker.controls.push_back(control);
00401   return int_marker;
00402 }
00403 
00404 
00405 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00406                                                          float scale, std_msgs::ColorRGBA color, float angle, bool view_facing )
00407 {
00408   visualization_msgs::InteractiveMarker int_marker;
00409   int_marker.header =  stamped.header;
00410   int_marker.name = name;
00411   int_marker.scale = scale;
00412   int_marker.pose = stamped.pose;
00413 
00414 //  add6DofControl(int_marker, false);
00415 
00416   visualization_msgs::InteractiveMarkerControl control;
00417 
00418   visualization_msgs::Marker mesh;
00419   mesh.mesh_use_embedded_materials = false;
00420   mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
00421   mesh.scale.x = 1.0;
00422   mesh.scale.y = 1.0;
00423   mesh.scale.z = 1.0;
00424   mesh.color.r = color.r;
00425   mesh.color.g = color.g;
00426   mesh.color.b = color.b;
00427   mesh.color.a = color.a;
00428 
00429   tf::Transform T1, T2;
00430   tf::Transform T_proximal, T_distal;
00431 
00432   T1.setOrigin(tf::Vector3(0.07691, 0.01, 0));
00433   T1.setRotation(tf::Quaternion(tf::Vector3(0,0,1),  angle));
00434   T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0));
00435   T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle));
00436   T_proximal = T1;
00437   T_distal = T1 * T2;
00438 
00439   mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae";
00440   mesh.pose.orientation.w = 1;
00441   control.markers.push_back( mesh );
00442   mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae";
00443   mesh.pose = object_manipulator::msg::createPoseMsg(T_proximal);
00444   control.markers.push_back( mesh );
00445   mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae";
00446   mesh.pose = object_manipulator::msg::createPoseMsg(T_distal);
00447   control.markers.push_back( mesh );
00448 
00449   T1.setOrigin(tf::Vector3(0.07691, -0.01, 0));
00450   T1.setRotation(tf::Quaternion(tf::Vector3(1,0,0), M_PI)*tf::Quaternion(tf::Vector3(0,0,1),  angle));
00451   T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0));
00452   T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle));
00453   T_proximal = T1;
00454   T_distal = T1 * T2;
00455 
00456   mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae";
00457   mesh.pose = object_manipulator::msg::createPoseMsg(T_proximal);
00458   control.markers.push_back( mesh );
00459   mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae";
00460   mesh.pose = object_manipulator::msg::createPoseMsg(T_distal);
00461   control.markers.push_back( mesh );
00462 
00463   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00464   int_marker.controls.push_back( control );
00465 
00466   return int_marker;
00467 }
marker_helpers.cpp
