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