Merge pull request #49 from ctu-mrs/batch_visualizer

add the option to visualize mrs_msgs::Path directly

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/Path.h>
 #include <mrs_msgs/TrajectoryReference.h>
 #include <mrs_lib/visual_object.h>
 #include <set>
@@ -157,6 +158,20 @@ class BatchVisualizer {
   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));
 
+  /**
+   * @brief add a path to the buffer
+   *
+   * @param p path 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
+   * @param timeout time in seconds after which the object should be removed from buffer
+   */
+  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));
+
   /**
    * @brief add a trajectory to the buffer
    *
@@ -226,10 +241,10 @@ class BatchVisualizer {
   ros::Publisher                  visual_pub;
   visualization_msgs::MarkerArray msg;
 
-  std::string parent_frame; // coordinate frame id
+  std::string parent_frame;  // coordinate frame id
   std::string marker_topic_name;
 
-  std::set<VisualObject> visual_objects; // buffer for objects to be visualized
+  std::set<VisualObject> visual_objects;  // buffer for objects to be visualized
 
   visualization_msgs::Marker points_marker;
   visualization_msgs::Marker lines_marker;
@@ -241,7 +256,7 @@ class BatchVisualizer {
   double points_scale = 0.02;
   double lines_scale  = 0.04;
 
-  unsigned long uuid = 0; // create unique ID for items in object buffer
+  unsigned long uuid = 0;  // create unique ID for items in object buffer
 };
 
 }  // namespace mrs_lib

include/mrs_lib/visual_object.h

@@ -11,6 +11,7 @@
 #include <std_msgs/ColorRGBA.h>
 #include <geometry_msgs/Point.h>
 #include <mrs_lib/geometry/shapes.h>
+#include <mrs_msgs/Path.h>
 #include <mrs_msgs/TrajectoryReference.h>
 
 #define DEFAULT_ELLIPSE_POINTS 64
@@ -53,6 +54,9 @@ class VisualObject {
   VisualObject(const mrs_lib::geometry::Cone& cone, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
                const bool filled, const bool capped, const unsigned long& id, const int num_sides = DEFAULT_ELLIPSE_POINTS);
 
+  VisualObject(const mrs_msgs::Path& p, const double r, const double g, const double b, const double a, const ros::Duration& timeout, const bool filled,
+               const unsigned long& id);
+
   VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
                const bool filled, const unsigned long& id);
 

src/batch_visualizer/batch_visualizer.cpp

@@ -157,6 +157,14 @@ void BatchVisualizer::addCone(const mrs_lib::geometry::Cone &cone, const double
 }
 //}
 
+/* addPath //{ */
+void BatchVisualizer::addPath(const mrs_msgs::Path &p, const double r, const double g, const double b, const double a, const bool filled,
+                              const ros::Duration &timeout) {
+  VisualObject obj = VisualObject(p, r, g, b, a, timeout, filled, uuid++);
+  visual_objects.insert(obj);
+}
+//}
+
 /* addTrajectory //{ */
 void BatchVisualizer::addTrajectory(const mrs_msgs::TrajectoryReference &traj, const double r, const double g, const double b, const double a,
                                     const bool filled, const ros::Duration &timeout) {

src/batch_visualizer/visual_object.cpp

@@ -250,6 +250,40 @@ VisualObject::VisualObject(const mrs_lib::geometry::Cone& cone, const double r,
 }
 //}
 
+/* mrs_msgs::Path //{ */
+VisualObject::VisualObject(const mrs_msgs::Path& p, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+                           const bool filled, const unsigned long& id)
+    : id_(id) {
+  if (p.points.size() < 2) {
+    return;
+  }
+  if (filled) {
+    for (size_t i = 0; i < p.points.size() - 1; i++) {
+      Eigen::Vector3d p1, p2;
+      p1.x()   = p.points[i].position.x;
+      p1.y()   = p.points[i].position.y;
+      p1.z()   = p.points[i].position.z;
+      p2.x()   = p.points[i + 1].position.x;
+      p2.y()   = p.points[i + 1].position.y;
+      p2.z()   = p.points[i + 1].position.z;
+      auto ray = mrs_lib::geometry::Ray::twopointCast(p1, p2);
+      addRay(ray, r, g, b, a);
+    }
+  } else {
+    type_ = MarkerType::POINT;
+    for (size_t i = 0; i < p.points.size(); i++) {
+      points_.push_back(p.points[i].position);
+      colors_.push_back(generateColor(r, g, b, a));
+    }
+  }
+  if (timeout.toSec() <= 0) {
+    timeout_time_ = ros::Time(0);
+  } else {
+    timeout_time_ = ros::Time::now() + timeout;
+  }
+}
+//}
+
 /* mrs_msgs::TrajectoryReference //{ */
 VisualObject::VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a,
                            const ros::Duration& timeout, const bool filled, const unsigned long& id)