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 
00054 void add6DofControl( visualization_msgs::InteractiveMarker &msg, bool fixed = false );
00055 
00056 
00057 visualization_msgs::InteractiveMarkerControl& makeBoxControl( visualization_msgs::InteractiveMarker &msg );
00058 
00059 visualization_msgs::InteractiveMarkerControl& makeSphereControl( visualization_msgs::InteractiveMarker &msg );
00060 
00061 visualization_msgs::MenuEntry makeMenuEntry(const char *title);
00062 
00063 visualization_msgs::MenuEntry makeMenuEntry(const char *title, const char *command, int type  );
00064 
00065 visualization_msgs::InteractiveMarker makePostureMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 
00066                                                          float scale, bool fixed, bool view_facing );
00067 
00068 visualization_msgs::InteractiveMarker makeHeadGoalMarker( const char *name, const geometry_msgs::PoseStamped &stamped, 
00069                                                           float scale);
00070 
00071 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00072                                                       const geometry_msgs::PoseStamped &stamped, float scale );
00073 
00074 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00075                                                       const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color );
00076 
00077 visualization_msgs::InteractiveMarker makeMeshMarker( const std::string &name, const std::string &mesh_resource,
00078                                                       const geometry_msgs::PoseStamped &stamped, float scale, const std_msgs::ColorRGBA &color, bool use_color );
00079 
00080 visualization_msgs::InteractiveMarker makeButtonBox( const char *name, const geometry_msgs::PoseStamped &stamped,
00081                                                      float scale, bool fixed, bool view_facing );
00082 
00083 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped,
00084                                                      float scale, bool fixed, bool view_facing );
00085 
00086 visualization_msgs::InteractiveMarker makeButtonSphere( const char *name, const geometry_msgs::PoseStamped &stamped,
00087                                                      float scale, bool fixed, bool view_facing, std_msgs::ColorRGBA color );
00088 
00089 visualization_msgs::InteractiveMarker makeListControl( const char *name, const geometry_msgs::PoseStamped &stamped, int num, int total, float scale);
00090 
00091 visualization_msgs::InteractiveMarker make6DofMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00092                                                       float scale, bool fixed, bool view_facing );
00093 
00094 visualization_msgs::InteractiveMarker makePlanarMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00095                                                       float scale, bool fixed );
00096 
00097 visualization_msgs::InteractiveMarker makeElevatorMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00098                                                           float scale, bool fixed);
00099 
00100 visualization_msgs::InteractiveMarker makeProjectorMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00101                                                            float scale);
00102 
00103 
00104 visualization_msgs::InteractiveMarker makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00105                                                       float scale, bool fixed);
00106 
00107 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00108                                                          float scale, float angle, bool view_facing );
00109 
00110 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00111                                                          float scale, float angle, bool view_facing, std_msgs::ColorRGBA color );
00112 
00113 visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
00114                                                          float scale, float angle, bool view_facing, std_msgs::ColorRGBA color, bool use_color );
00115 
00116 visualization_msgs::InteractiveMarker makeGraspMarker( const char * name, const geometry_msgs::PoseStamped &stamped, float scale, PoseState pose_state);
00117 
00118 visualization_msgs::InteractiveMarker makePosedMultiMeshMarker( const char * name, const geometry_msgs::PoseStamped &stamped,
00119                                                             const std::vector< geometry_msgs::PoseStamped> &mesh_poses,
00120                                                             const std::vector<std::string> &mesh_paths, const float &scale, const bool button_only = true);
00121 
00122 visualization_msgs::InteractiveMarker makeFollowerMultiMeshMarker( const char * name, const geometry_msgs::PoseStamped &stamped,
00123                                                                    const std::vector<std::string> &mesh_frames,
00124                                                                    const std::vector<std::string> &mesh_paths,
00125                                                                    const float &scale);
00126 
00127 
00128 
00129 } 
00130 
00131 #endif