[BatchVisualizer]: add trajectory visualization

include/mrs_lib/batch_visualizer.h

@@ -10,6 +10,7 @@
 #include <ros/ros.h>
 #include <visualization_msgs/MarkerArray.h>
 #include <mrs_lib/geometry/shapes.h>
+#include <mrs_msgs/TrajectoryReference.h>
 /* #include <dynamic_reconfigure/server.h> */
 /* #include <mrs_lib/batch_visualizerConfig.h> */
 
@@ -142,13 +143,25 @@ class BatchVisualizer {
   void addCone(mrs_lib::geometry::Cone cone, double r = 0.7, double g = 0.8, double b = 0.3, double a = 1.0, bool filled = true, bool capped = true,
                int sides = DEFAULT_ELLIPSE_POINTS);
 
+  /**
+   * @brief add a trajectory to the buffer
+   *
+   * @param traj trajectory reference to be added
+   * @param r red color in range <0,1>
+   * @param g green color in range <0,1>
+   * @param b blue color in range <0,1>
+   * @param a alpha in range <0,1> (0 is fully transparent)
+   * @param filled bool to set fill. True = continuous line, False = only visualize points
+   */
+  void addTrajectory(mrs_msgs::TrajectoryReference traj, double r = 0.3, double g = 1.0, double b = 0.3, double a = 1.0, bool filled = true);
+
   /**
    * @brief set the scale of all points
    *
    * @param scale
    */
   void setPointsScale(double scale);
-  
+
   /**
    * @brief set the parent frame_id
    *

src/batch_visualizer/batch_visualizer.cpp

@@ -278,6 +278,33 @@ void BatchVisualizer::addCone(mrs_lib::geometry::Cone cone, double r, double g,
 }
 //}
 
+/* addTrajectory //{ */
+void BatchVisualizer::addTrajectory(mrs_msgs::TrajectoryReference traj, double r, double g, double b, double a, bool filled) {
+  if (traj.points.size() < 2) {
+    ROS_WARN("[%s]: Trajectory too short to visualize!", ros::this_node::getName().c_str());
+    return;
+  }
+  if (filled) {
+    for (size_t i = 0; i < traj.points.size() - 1; i++) {
+      Eigen::Vector3d p1, p2;
+      p1.x()   = traj.points[i].position.x;
+      p1.y()   = traj.points[i].position.y;
+      p1.z()   = traj.points[i].position.z;
+      p2.x()   = traj.points[i + 1].position.x;
+      p2.y()   = traj.points[i + 1].position.y;
+      p2.z()   = traj.points[i + 1].position.z;
+      auto ray = mrs_lib::geometry::Ray::twopointCast(p1, p2);
+      addRay(ray, r, g, b, a);
+    }
+  } else {
+    for (size_t i = 0; i < traj.points.size(); i++) {
+      Eigen::Vector3d p(traj.points[i].position.x, traj.points[i].position.y, traj.points[i].position.z);
+      addPoint(p);
+    }
+  }
+}
+//}
+
 /* buildEllipse //{ */
 std::vector<Eigen::Vector3d> BatchVisualizer::buildEllipse(mrs_lib::geometry::Ellipse ellipse, int num_points) {
   std::vector<Eigen::Vector3d> points;