visual_object.h

#ifndef BATCH_VISUALIZER__VISUAL_OBJECT_H
#define BATCH_VISUALIZER__VISUAL_OBJECT_H
 
#include <rclcpp/rclcpp.hpp>
#include <Eigen/Core>
#include <std_msgs/msg/color_rgba.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <mrs_lib/geometry/shapes.h>
#include <mrs_msgs/msg/path.hpp>
#include <mrs_msgs/msg/trajectory_reference.hpp>
 
#define DEFAULT_ELLIPSE_POINTS 64
 
namespace mrs_lib
{
 
  enum MarkerType
  {
    POINT = 0,
    LINE = 1,
    TRIANGLE = 2
  };
 
  class VisualObject
  {
 
 
  public:
    VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
    VisualObject(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
    VisualObject(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
    VisualObject(const mrs_lib::geometry::Rectangle& rectangle, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
    VisualObject(const mrs_lib::geometry::Cuboid& cuboid, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
    VisualObject(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const unsigned long& id, const rclcpp::Node::SharedPtr& node, const int num_points = DEFAULT_ELLIPSE_POINTS);
 
    VisualObject(const mrs_lib::geometry::Cylinder& cylinder, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const bool capped, const unsigned long& id, const rclcpp::Node::SharedPtr& node,
                 const int num_sides = DEFAULT_ELLIPSE_POINTS);
 
    VisualObject(const mrs_lib::geometry::Cone& cone, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const bool capped, const unsigned long& id, const rclcpp::Node::SharedPtr& node,
                 const int num_sides = DEFAULT_ELLIPSE_POINTS);
 
    VisualObject(const mrs_msgs::msg::Path& p, const double r, const double g, const double b, const double a, const rclcpp::Duration& timeout,
                 const bool filled, const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
    VisualObject(const mrs_msgs::msg::TrajectoryReference& traj, const double r, const double g, const double b, const double a,
                 const rclcpp::Duration& timeout, const bool filled, const unsigned long& id, const rclcpp::Node::SharedPtr& node);
 
 
  public:
    unsigned long getID() const;
    int getType() const;
    bool isTimedOut() const;
 
    const std::vector<geometry_msgs::msg::Point> getPoints() const;
    const std::vector<std_msgs::msg::ColorRGBA> getColors() const;
 
    bool operator<(const VisualObject& other) const
    {
      return id_ < other.id_;
    }
 
    bool operator>(const VisualObject& other) const
    {
      return id_ > other.id_;
    }
 
    bool operator==(const VisualObject& other) const
    {
      return id_ == other.id_;
    }
 
  private:
    unsigned long id_;
    MarkerType type_;
    std::vector<geometry_msgs::msg::Point> points_;
    std::vector<std_msgs::msg::ColorRGBA> colors_;
    rclcpp::Time timeout_time_;
    rclcpp::Node::SharedPtr node_;
 
    void addRay(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a);
 
    void addTriangle(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const bool filled);
 
    void addEllipse(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a, const bool filled,
                    const int num_points);
 
  }; // namespace batch_visualizer
 
} // namespace mrs_lib
 
#endif