batch_visualizer.h

Go to the documentation of this file.

 
#ifndef BATCH_VISUALIZER_H
#define BATCH_VISUALIZER_H
 
#include <vector>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <mrs_lib/geometry/shapes.h>
#include <mrs_msgs/Path.h>
#include <mrs_msgs/TrajectoryReference.h>
#include <mrs_lib/visual_object.h>
#include <set>
 
#define DEFAULT_ELLIPSE_POINTS 64
 
namespace mrs_lib
{
 
class BatchVisualizer {
 
public:
  BatchVisualizer();
  ~BatchVisualizer();
  BatchVisualizer(ros::NodeHandle &nh, 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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(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 ros::Duration &timeout = ros::Duration(0));
 
  void addPath(const mrs_msgs::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 ros::Duration &timeout = ros::Duration(0));
 
  void addTrajectory(const mrs_msgs::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 ros::Duration &timeout = ros::Duration(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();
 
private:
  ros::Publisher                  visual_pub;
  visualization_msgs::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::Marker points_marker;
  visualization_msgs::Marker lines_marker;
  visualization_msgs::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