From e29a257c53b217550d9ebb61501b9675a5a29e8a Mon Sep 17 00:00:00 2001 From: stibipet Date: Fri, 15 Mar 2024 13:29:57 +0100 Subject: [PATCH] add the option to visualize mrs_msgs::Path directly --- include/mrs_lib/batch_visualizer.h | 21 ++++++++++++-- include/mrs_lib/visual_object.h | 4 +++ src/batch_visualizer/batch_visualizer.cpp | 8 ++++++ src/batch_visualizer/visual_object.cpp | 34 +++++++++++++++++++++++ 4 files changed, 64 insertions(+), 3 deletions(-) diff --git a/include/mrs_lib/batch_visualizer.h b/include/mrs_lib/batch_visualizer.h index 2531659f..22296906 100644 --- a/include/mrs_lib/batch_visualizer.h +++ b/include/mrs_lib/batch_visualizer.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -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 visual_objects; // buffer for objects to be visualized + std::set 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 diff --git a/include/mrs_lib/visual_object.h b/include/mrs_lib/visual_object.h index 49e30c26..a55d18b0 100644 --- a/include/mrs_lib/visual_object.h +++ b/include/mrs_lib/visual_object.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #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); diff --git a/src/batch_visualizer/batch_visualizer.cpp b/src/batch_visualizer/batch_visualizer.cpp index 6692b2c1..cc5d5276 100644 --- a/src/batch_visualizer/batch_visualizer.cpp +++ b/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) { diff --git a/src/batch_visualizer/visual_object.cpp b/src/batch_visualizer/visual_object.cpp index 4c165456..b7e8b376 100644 --- a/src/batch_visualizer/visual_object.cpp +++ b/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)