batch_visualizer.h

#ifndef BATCH_VISUALIZER_H
#define BATCH_VISUALIZER_H
 
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <mrs_lib/geometry/shapes.h>
#include <mrs_msgs/msg/path.hpp>
#include <mrs_msgs/msg/trajectory_reference.hpp>
#include <mrs_lib/visual_object.h>
#include <set>
 
#define DEFAULT_ELLIPSE_POINTS 64
 
namespace mrs_lib
{
 
  class BatchVisualizer
  {
 
  public:
    BatchVisualizer();
    ~BatchVisualizer();
    BatchVisualizer(const std::shared_ptr<rclcpp::Node>& node, std::string marker_topic_name, std::string parent_frame);
 
    void addPoint(const Eigen::Vector3d& point, const double r = 0.0, const double g = 1.0, const double b = 0.3, const double a = 1.0,
                  const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addRay(const mrs_lib::geometry::Ray& ray, const double r = 1.0, const double g = 0.0, const double b = 0.0, const double a = 1.0,
                const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addTriangle(const mrs_lib::geometry::Triangle& tri, const double r = 0.5, const double g = 0.5, const double b = 0.0, const double a = 1.0,
                     const bool filled = true, const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addRectangle(const mrs_lib::geometry::Rectangle& rect, const double r = 0.5, const double g = 0.5, const double b = 0.0, const double a = 1.0,
                      const bool filled = true, const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addCuboid(const mrs_lib::geometry::Cuboid& cuboid, const double r = 0.5, const double g = 0.5, const double b = 0.0, const double a = 1.0,
                   const bool filled = true, const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addEllipse(const mrs_lib::geometry::Ellipse& ellipse, const double r = 0.0, const double g = 1.0, const double b = 1.0, const double a = 1.0,
                    const bool filled = true, const int num_points = DEFAULT_ELLIPSE_POINTS,
                    const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addCylinder(const mrs_lib::geometry::Cylinder& cylinder, const double r = 0.7, const double g = 0.8, const double b = 0.3, const double a = 1.0,
                     const bool filled = true, const bool capped = true, const int sides = DEFAULT_ELLIPSE_POINTS,
                     const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
    void addCone(const mrs_lib::geometry::Cone& cone, const double r = 0.7, const double g = 0.8, const double b = 0.3, const double a = 1.0,
                 const bool filled = true, const bool capped = true, const int sides = DEFAULT_ELLIPSE_POINTS,
                 const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addPath(const mrs_msgs::msg::Path& p, const double r = 0.3, const double g = 1.0, const double b = 0.3, const double a = 1.0, const bool filled = true,
                 const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addTrajectory(const mrs_msgs::msg::TrajectoryReference& traj, const double r = 0.3, const double g = 1.0, const double b = 0.3, const double a = 1.0,
                       const bool filled = true, const rclcpp::Duration& timeout = rclcpp::Duration(std::chrono::seconds(0)));
 
    void addNullPoint();
 
    void addNullLine();
 
    void addNullTriangle();
 
    void setPointsScale(const double scale);
 
    void setParentFrame(const std::string parent_frame);
 
    void setLinesScale(const double scale);
 
    void clearBuffers();
 
    void clearVisuals();
 
    void publish();
 
    void publish(const rclcpp::Time stamp);
 
  private:
    std::shared_ptr<rclcpp::Node> node;
    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr visual_pub;
    visualization_msgs::msg::MarkerArray msg;
 
    std::string parent_frame; // coordinate frame id
    std::string marker_topic_name;
 
    std::set<VisualObject> visual_objects; // buffer for objects to be visualized
 
    visualization_msgs::msg::Marker points_marker;
    visualization_msgs::msg::Marker lines_marker;
    visualization_msgs::msg::Marker triangles_marker;
 
    bool initialized = false;
    void initialize();
 
    double points_scale = 0.02;
    double lines_scale = 0.04;
 
    unsigned long uuid = 0; // create unique ID for items in object buffer
  };
 
} // namespace mrs_lib
 
#endif