Completing Hybrid-A* visualization of expansion footprints PR (ros-na…

…vigation#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/src/smac_planner_hybrid.cpp

@@ -220,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);
   }
 
   if (_debug_visualizations) {
@@ -381,20 +384,6 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     }
   }
 
-  // Publish expansions for debug
-  if (_viz_expansions) {
-    geometry_msgs::msg::PoseArray msg;
-    geometry_msgs::msg::Pose msg_pose;
-    msg.header.stamp = _clock->now();
-    msg.header.frame_id = _global_frame;
-    for (auto & e : *expansions) {
-      msg_pose.position.x = std::get<0>(e);
-      msg_pose.position.y = std::get<1>(e);
-      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) {