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 #ifndef _MARKER_HELPERS_H_
00033 #define _MARKER_HELPERS_H_
00034 
00035 #include <interactive_markers/tools.h>
00036 #include <visualization_msgs/InteractiveMarker.h>
00037 #include <visualization_msgs/InteractiveMarkerControl.h>
00038 #include <visualization_msgs/Marker.h>
00039 #include <visualization_msgs/MenuEntry.h>
00040 #include <geometry_msgs/PoseStamped.h>
00041 
00042 
00043 
00044 namespace im_helpers {
00045 
00046 enum PoseState {UNTESTED, VALID, INVALID};
00047 
00048 visualization_msgs::InteractiveMarker makeEmptyMarker( const char *frame_id = "" );
00049 
00050 visualization_msgs::Marker makeBox( float scale );
00051 
00052 visualization_msgs::Marker makeSphere( float scale );
00053 void add3Dof2DControl( visualization_msgs::InteractiveMarker &msg, bool fixed = false);
00054 void add6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed = false );
00055 void addVisible6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed = false, bool visible = true );
00056 
00057 
00058 visualization_msgs::InteractiveMarkerControl& makeBoxControl( visualization_msgs::InteractiveMarker &msg );
00059 
00060 visualization_msgs::InteractiveMarkerControl& makeSphereControl( visualization_msgs::InteractiveMarker &msg );
00061 
00062 visualization_msgs::MenuEntry makeMenuEntry(const char *title);
00063 
00064 visualization_msgs::MenuEntry makeMenuEntry(const char *title, const char *command, int type  );
00065 
00066 visualization_msgs::InteractiveMarker makePostureMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 
00067                                                          float scale, bool fixed, bool view_facing );
00068 
00069 visualization_msgs::InteractiveMarker makeHeadGoalMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 
00070                                                           float scale);
00071 
00072 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00073                                                       const geometry_msgs::PoseStamped &stamped, float scale );
00074 
00075 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00076                                                       const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color );
00077 
00078 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00079                                                       const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color, bool use_color );
00080 
00081 visualization_msgs::InteractiveMarker makeButtonBox( const char *name, const geometry_msgs::PoseStamped &stamped,
00082                                                      float scale, bool fixed, bool view_facing );
00083 
00084 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped,
00085                                                      float scale, bool fixed, bool view_facing );
00086 
00087 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped,
00088                                                      float scale, bool fixed, bool view_facing, std_msgs::ColorRGBA color );
00089 
00090 visualization_msgs::InteractiveMarker makeListControl( const char *name, const geometry_msgs::PoseStamped &stamped, int num, int total, float scale);
00091 
00092 visualization_msgs::InteractiveMarker make6DofMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00093                                                       float scale, bool fixed, bool view_facing );
00094 
00095 visualization_msgs::InteractiveMarker makePlanarMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00096                                                       float scale, bool fixed );
00097 
00098 visualization_msgs::InteractiveMarker makeElevatorMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00099                                                           float scale, bool fixed);
00100 
00101 visualization_msgs::InteractiveMarker makeProjectorMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00102                                                            float scale);
00103 
00104 
00105 visualization_msgs::InteractiveMarker makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00106                                                       float scale, bool fixed);
00107 
00108 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00109                                                          float scale, float angle, bool view_facing );
00110 
00111 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00112                                                          float scale, float angle, bool view_facing, std_msgs::ColorRGBA color );
00113 
00114 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00115                                                          float scale, float angle, bool view_facing, std_msgs::ColorRGBA color, bool use_color );
00116 
00117 visualization_msgs::InteractiveMarker makeGraspMarker( const char * name, const geometry_msgs::PoseStamped &stamped, float scale, PoseState pose_state);
00118 
00119 visualization_msgs::InteractiveMarker makePosedMultiMeshMarker( const char * name, const geometry_msgs::PoseStamped &stamped,
00120                                                             const std::vector< geometry_msgs::PoseStamped> &mesh_poses,
00121                                                             const std::vector<std::string> &mesh_paths, const float &scale, const bool button_only = true);
00122 
00123 visualization_msgs::InteractiveMarker makeFollowerMultiMeshMarker( const char * name, const geometry_msgs::PoseStamped &stamped,
00124                                                                    const std::vector<std::string> &mesh_frames,
00125                                                                    const std::vector<std::string> &mesh_paths,
00126                                                                    const float &scale);
00127 
00128 
00129 
00130 } 
00131 
00132 #endif