marker_manager.hpp

Go to the documentation of this file.

#ifndef __MARKER_MANAGER__
#define __MARKER_MANAGER__

#include <eigen_conversions/eigen_msg.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <generic_control_toolbox/manager_base.hpp>

namespace generic_control_toolbox
{
enum MarkerType
{
  sphere,
  arrow
};
class MarkerManager : public ManagerBase
{
 public:
  MarkerManager();
  ~MarkerManager();

  bool addMarkerGroup(const std::string &group_key,
                      const std::string &topic_name);

  bool addMarker(const std::string &group_key, const std::string &marker_name,
                 const std::string &ns, const std::string &frame_id,
                 MarkerType type);

  bool setMarkerColor(const std::string &group_key,
                      const std::string &marker_name, double r, double g,
                      double b);

  bool setMarkerScale(const std::string &group_key,
                      const std::string &marker_name, double x, double y,
                      double z);

  bool setMarkerPoints(const std::string &group_key,
                       const std::string &marker_name,
                       const Eigen::Vector3d &initial_point,
                       const Eigen::Vector3d &final_point);

  bool setMarkerPose(const std::string &group_key,
                     const std::string &marker_name,
                     const Eigen::Affine3d &pose);

  void publishMarkers();

 private:
  std::map<std::string, std::map<std::string, int> > marker_map_;
  std::map<std::string, visualization_msgs::MarkerArray> marker_array_;
  std::map<std::string, std::shared_ptr<realtime_tools::RealtimePublisher<
                            visualization_msgs::MarkerArray> > >
      marker_pub_;
  ros::NodeHandle n_;

  bool getMarkerId(const std::string &group_key, const std::string &marker_name,
                   int &id) const;
};
}  // namespace generic_control_toolbox
#endif