00001 #ifndef __MARKER_MANAGER__ 00002 #define __MARKER_MANAGER__ 00003 00004 #include <eigen_conversions/eigen_msg.h> 00005 #include <realtime_tools/realtime_publisher.h> 00006 #include <ros/ros.h> 00007 #include <visualization_msgs/Marker.h> 00008 #include <visualization_msgs/MarkerArray.h> 00009 #include <generic_control_toolbox/manager_base.hpp> 00010 00011 namespace generic_control_toolbox 00012 { 00013 enum MarkerType 00014 { 00015 sphere, 00016 arrow 00017 }; 00021 class MarkerManager : public ManagerBase 00022 { 00023 public: 00024 MarkerManager(); 00025 ~MarkerManager(); 00026 00036 bool addMarkerGroup(const std::string &group_key, 00037 const std::string &topic_name); 00038 00050 bool addMarker(const std::string &group_key, const std::string &marker_name, 00051 const std::string &ns, const std::string &frame_id, 00052 MarkerType type); 00053 00061 bool setMarkerColor(const std::string &group_key, 00062 const std::string &marker_name, double r, double g, 00063 double b); 00064 00072 bool setMarkerScale(const std::string &group_key, 00073 const std::string &marker_name, double x, double y, 00074 double z); 00075 00086 bool setMarkerPoints(const std::string &group_key, 00087 const std::string &marker_name, 00088 const Eigen::Vector3d &initial_point, 00089 const Eigen::Vector3d &final_point); 00090 00100 bool setMarkerPose(const std::string &group_key, 00101 const std::string &marker_name, 00102 const Eigen::Affine3d &pose); 00103 00107 void publishMarkers(); 00108 00109 private: 00110 std::map<std::string, std::map<std::string, int> > marker_map_; 00111 std::map<std::string, visualization_msgs::MarkerArray> marker_array_; 00112 std::map<std::string, std::shared_ptr<realtime_tools::RealtimePublisher< 00113 visualization_msgs::MarkerArray> > > 00114 marker_pub_; 00115 ros::NodeHandle n_; 00116 00123 bool getMarkerId(const std::string &group_key, const std::string &marker_name, 00124 int &id) const; 00125 }; 00126 } // namespace generic_control_toolbox 00127 #endif