Completing Hybrid-A* visualization of expansion footprints PR (#3733)

* smach_planner_hybrid: add support visualization for hybrid Astar

* smac_planner_hyrid: revert some

* smach_planner_hybrid: improving code quality

* utils: add some useful functions

* utils: fix mistake

* nav2_smac_planner: fix format problem

* utils: fix format and revise functions

* smach_planner_hybrid: delete _viz_expansion parameter

* smac_planner_hybrid: fix format

* README: update parameter

* utils: corrct mistake return

* utils: make timestamp a const reference

* nav2_smac_planner: correct format problem

* add unit test functions

* further detection of element equality

* test_utils: add non-trival translation and rotation

* smac_planner_hybrid: pass value instead of references

* completing hybrid A* visualization

---------

Co-authored-by: xianglunkai <[email protected]>

nav2_smac_planner/README.md

@@ -126,7 +126,7 @@ planner_server:
       cache_obstacle_heuristic: True      # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.  
       allow_reverse_expansion: False      # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.   
       smooth_path: True                   # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
-      viz_expansions: True                # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance. 
+      debug_visualizations: True                # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. 
       smoother:
         max_iterations: 1000
         w_smooth: 0.3

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -117,10 +117,12 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   double _max_planning_time;
   double _lookup_table_size;
   double _minimum_turning_radius_global_coords;
-  bool _viz_expansions;
+  bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    _planned_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
   std::mutex _mutex;

nav2_smac_planner/include/nav2_smac_planner/utils.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2021, Samsung Research America
+// Copyright (c) 2023, Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -17,13 +18,18 @@
 
 #include <vector>
 #include <memory>
+#include <string>
 
+#include "nlohmann/json.hpp"
 #include "Eigen/Core"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "tf2/utils.h"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include <rclcpp/rclcpp.hpp>
 
 namespace nav2_smac_planner
 {
@@ -154,6 +160,76 @@ inline void fromJsonToMotionPrimitive(
   }
 }
 
+/**
+ * @brief transform footprint into edges
+ * @param[in] robot position , orientation and  footprint
+ * @param[out] robot footprint edges
+ */
+inline std::vector<geometry_msgs::msg::Point> transformFootprintToEdges(
+  const geometry_msgs::msg::Pose & pose,
+  const std::vector<geometry_msgs::msg::Point> & footprint)
+{
+  const double & x = pose.position.x;
+  const double & y = pose.position.y;
+  const double & yaw = tf2::getYaw(pose.orientation);
+
+  std::vector<geometry_msgs::msg::Point> out_footprint;
+  out_footprint.resize(2 * footprint.size());
+  for (unsigned int i = 0; i < footprint.size(); i++) {
+    out_footprint[2 * i].x = x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y;
+    out_footprint[2 * i].y = y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y;
+    if (i == 0) {
+      out_footprint.back().x = out_footprint[i].x;
+      out_footprint.back().y = out_footprint[i].y;
+    } else {
+      out_footprint[2 * i - 1].x = out_footprint[2 * i].x;
+      out_footprint[2 * i - 1].y = out_footprint[2 * i].y;
+    }
+  }
+  return out_footprint;
+}
+
+/**
+ * @brief initializes marker to visualize shape of linestring
+ * @param edge       edge to mark of footprint
+ * @param i          marker ID
+ * @param frame_id   frame of the marker
+ * @param timestamp  timestamp of the marker
+ * @return marker populated
+ */
+inline visualization_msgs::msg::Marker createMarker(
+  const std::vector<geometry_msgs::msg::Point> edge,
+  unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp)
+{
+  visualization_msgs::msg::Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = timestamp;
+  marker.frame_locked = false;
+  marker.ns = "planned_footprint";
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+  marker.lifetime = rclcpp::Duration(0, 0);
+
+  marker.id = i;
+  for (auto & point : edge) {
+    marker.points.push_back(point);
+  }
+
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 0.05;
+  marker.scale.y = 0.05;
+  marker.scale.z = 0.05;
+  marker.color.r = 0.0f;
+  marker.color.g = 0.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 1.3f;
+  return marker;
+}
+
+
 }  // namespace nav2_smac_planner
 
 #endif  // NAV2_SMAC_PLANNER__UTILS_HPP_

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2020, Samsung Research America
+// Copyright (c) 2023, Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -134,8 +135,8 @@ void SmacPlannerHybrid::configure(
   node->get_parameter(name + ".lookup_table_size", _lookup_table_size);
 
   nav2_util::declare_parameter_if_not_declared(
-    node, name + ".viz_expansions", rclcpp::ParameterValue(false));
-  node->get_parameter(name + ".viz_expansions", _viz_expansions);
+    node, name + ".debug_visualizations", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".debug_visualizations", _debug_visualizations);
 
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
@@ -219,8 +220,11 @@ void SmacPlannerHybrid::configure(
   }
 
   _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
-  if (_viz_expansions) {
+
+  if (_debug_visualizations) {
     _expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
+    _planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "planned_footprints", 1);
   }
 
   RCLCPP_INFO(
@@ -238,8 +242,9 @@ void SmacPlannerHybrid::activate()
     _logger, "Activating plugin %s of type SmacPlannerHybrid",
     _name.c_str());
   _raw_plan_publisher->on_activate();
-  if (_viz_expansions) {
+  if (_debug_visualizations) {
     _expansions_publisher->on_activate();
+    _planned_footprints_publisher->on_activate();
   }
   if (_costmap_downsampler) {
     _costmap_downsampler->on_activate();
@@ -256,8 +261,9 @@ void SmacPlannerHybrid::deactivate()
     _logger, "Deactivating plugin %s of type SmacPlannerHybrid",
     _name.c_str());
   _raw_plan_publisher->on_deactivate();
-  if (_viz_expansions) {
+  if (_debug_visualizations) {
     _expansions_publisher->on_deactivate();
+    _planned_footprints_publisher->on_activate();
   }
   if (_costmap_downsampler) {
     _costmap_downsampler->on_deactivate();
@@ -278,6 +284,7 @@ void SmacPlannerHybrid::cleanup()
   }
   _raw_plan_publisher.reset();
   _expansions_publisher.reset();
+  _planned_footprints_publisher.reset();
 }
 
 nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
@@ -352,7 +359,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   int num_iterations = 0;
   std::string error;
   std::unique_ptr<std::vector<std::tuple<float, float>>> expansions = nullptr;
-  if (_viz_expansions) {
+  if (_debug_visualizations) {
     expansions = std::make_unique<std::vector<std::tuple<float, float>>>();
   }
   // Note: All exceptions thrown are handled by the planner server and returned to the action
@@ -367,8 +374,21 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     }
   }
 
-  // Publish expansions for debug
-  if (_viz_expansions) {
+  // Convert to world coordinates
+  plan.poses.reserve(path.size());
+  for (int i = path.size() - 1; i >= 0; --i) {
+    pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
+    pose.pose.orientation = getWorldOrientation(path[i].theta);
+    plan.poses.push_back(pose);
+  }
+
+  // Publish raw path for debug
+  if (_raw_plan_publisher->get_subscription_count() > 0) {
+    _raw_plan_publisher->publish(plan);
+  }
+
+  if (_debug_visualizations) {
+    // Publish expansions for debug
     geometry_msgs::msg::PoseArray msg;
     geometry_msgs::msg::Pose msg_pose;
     msg.header.stamp = _clock->now();
@@ -379,19 +399,23 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
       msg.poses.push_back(msg_pose);
     }
     _expansions_publisher->publish(msg);
-  }
 
-  // Convert to world coordinates
-  plan.poses.reserve(path.size());
-  for (int i = path.size() - 1; i >= 0; --i) {
-    pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
-    pose.pose.orientation = getWorldOrientation(path[i].theta);
-    plan.poses.push_back(pose);
-  }
+    // plot footprint path planned for debug
+    if (_planned_footprints_publisher->get_subscription_count() > 0) {
+      auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      for (size_t i = 0; i < plan.poses.size(); i++) {
+        const std::vector<geometry_msgs::msg::Point> edge =
+          transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
+        marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
+      }
 
-  // Publish raw path for debug
-  if (_raw_plan_publisher->get_subscription_count() > 0) {
-    _raw_plan_publisher->publish(plan);
+      if (marker_array->markers.empty()) {
+        visualization_msgs::msg::Marker clear_all_marker;
+        clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
+        marker_array->markers.push_back(clear_all_marker);
+      }
+      _planned_footprints_publisher->publish(std::move(marker_array));
+    }
   }
 
   // Find how much time we have left to do smoothing

nav2_smac_planner/test/CMakeLists.txt

@@ -1,3 +1,14 @@
+# Test utils
+ament_add_gtest(test_utils
+  test_utils.cpp
+)
+ament_target_dependencies(test_utils
+  ${dependencies}
+)
+target_link_libraries(test_utils
+  ${library_name}
+)
+
 # Test costmap downsampler
 ament_add_gtest(test_costmap_downsampler
   test_costmap_downsampler.cpp