Publish planned footprints after smoothing (#5155)

* Publish planned footprints after smoothing

Signed-off-by: Tony Najjar <[email protected]>

* Revert "Publish planned footprints after smoothing"

This reverts commit c9b349a.

* Add smoothed footprints publishing

Signed-off-by: Tony Najjar <[email protected]>

* fix formatting

Signed-off-by: Tony Najjar <[email protected]>

* Fix indentation for publisher creation in SmacPlannerHybrid and SmacPlannerLattice

Signed-off-by: Tony Najjar <[email protected]>

* address PR comments

Signed-off-by: Tony Najjar <[email protected]>

* fixes

Signed-off-by: Tony Najjar <[email protected]>

* fix build error

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>

Author: tonynajjar

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -128,6 +128,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   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<visualization_msgs::msg::MarkerArray>::SharedPtr
+    _smoothed_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
   std::mutex _mutex;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -118,6 +118,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
   bool _debug_visualizations;
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     _planned_footprints_publisher;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    _smoothed_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
   GoalHeadingMode _goal_heading_mode;

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -294,6 +294,9 @@ void SmacPlannerHybrid::configure(
     _expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
     _planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
       "planned_footprints", 1);
+    _smoothed_footprints_publisher =
+      node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "smoothed_footprints", 1);
   }
 
   RCLCPP_INFO(
@@ -314,6 +317,7 @@ void SmacPlannerHybrid::activate()
   if (_debug_visualizations) {
     _expansions_publisher->on_activate();
     _planned_footprints_publisher->on_activate();
+    _smoothed_footprints_publisher->on_activate();
   }
   if (_costmap_downsampler) {
     _costmap_downsampler->on_activate();
@@ -342,6 +346,7 @@ void SmacPlannerHybrid::deactivate()
   if (_debug_visualizations) {
     _expansions_publisher->on_deactivate();
     _planned_footprints_publisher->on_deactivate();
+    _smoothed_footprints_publisher->on_deactivate();
   }
   if (_costmap_downsampler) {
     _costmap_downsampler->on_deactivate();
@@ -367,8 +372,11 @@ void SmacPlannerHybrid::cleanup()
     _costmap_downsampler.reset();
   }
   _raw_plan_publisher.reset();
-  _expansions_publisher.reset();
-  _planned_footprints_publisher.reset();
+  if (_debug_visualizations) {
+    _expansions_publisher.reset();
+    _planned_footprints_publisher.reset();
+    _smoothed_footprints_publisher.reset();
+  }
 }
 
 nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
@@ -522,9 +530,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
   if (_debug_visualizations) {
     // Publish expansions for debug
+    auto now = _clock->now();
     geometry_msgs::msg::PoseArray msg;
     geometry_msgs::msg::Pose msg_pose;
-    msg.header.stamp = _clock->now();
+    msg.header.stamp = now;
     msg.header.frame_id = _global_frame;
     for (auto & e : *expansions) {
       msg_pose.position.x = std::get<0>(e);
@@ -534,19 +543,20 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     }
     _expansions_publisher->publish(msg);
 
-    // plot footprint path planned for debug
     if (_planned_footprints_publisher->get_subscription_count() > 0) {
+      // Clear all markers first
       auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      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));
+
+      // Publish smoothed footprints for debug
+      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()));
-      }
-
-      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);
+        marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
       }
       _planned_footprints_publisher->publish(std::move(marker_array));
     }
@@ -574,6 +584,27 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     " milliseconds to smooth path." << std::endl;
 #endif
 
+  if (_debug_visualizations) {
+    if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
+      // Clear all markers first
+      auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      visualization_msgs::msg::Marker clear_all_marker;
+      clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
+      marker_array->markers.push_back(clear_all_marker);
+      _smoothed_footprints_publisher->publish(std::move(marker_array));
+
+      // Publish smoothed footprints for debug
+      marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      auto now = _clock->now();
+      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, now));
+      }
+      _smoothed_footprints_publisher->publish(std::move(marker_array));
+    }
+  }
+
   return plan;
 }
 

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -56,7 +56,6 @@ void SmacPlannerLattice::configure(
   _costmap_ros = costmap_ros;
   _name = name;
   _global_frame = costmap_ros->getGlobalFrameID();
-  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
 
   RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str());
 
@@ -238,10 +237,15 @@ void SmacPlannerLattice::configure(
     _smoother->initialize(_metadata.min_turning_radius);
   }
 
+  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
+
   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);
+    _smoothed_footprints_publisher =
+      node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "smoothed_footprints", 1);
   }
 
   RCLCPP_INFO(
@@ -262,6 +266,7 @@ void SmacPlannerLattice::activate()
   if (_debug_visualizations) {
     _expansions_publisher->on_activate();
     _planned_footprints_publisher->on_activate();
+    _smoothed_footprints_publisher->on_activate();
   }
   auto node = _node.lock();
   // Add callback for dynamic parameters
@@ -278,6 +283,7 @@ void SmacPlannerLattice::deactivate()
   if (_debug_visualizations) {
     _expansions_publisher->on_deactivate();
     _planned_footprints_publisher->on_deactivate();
+    _smoothed_footprints_publisher->on_deactivate();
   }
   // shutdown dyn_param_handler
   auto node = _node.lock();
@@ -296,6 +302,11 @@ void SmacPlannerLattice::cleanup()
   _a_star.reset();
   _smoother.reset();
   _raw_plan_publisher.reset();
+  if (_debug_visualizations) {
+    _expansions_publisher.reset();
+    _planned_footprints_publisher.reset();
+    _smoothed_footprints_publisher.reset();
+  }
 }
 
 nav_msgs::msg::Path SmacPlannerLattice::createPlan(
@@ -390,9 +401,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
       _tolerance / static_cast<float>(_costmap->getResolution()), cancel_checker, expansions.get()))
   {
     if (_debug_visualizations) {
+      auto now = _clock->now();
       geometry_msgs::msg::PoseArray msg;
       geometry_msgs::msg::Pose msg_pose;
-      msg.header.stamp = _clock->now();
+      msg.header.stamp = now;
       msg.header.frame_id = _global_frame;
       for (auto & e : *expansions) {
         msg_pose.position.x = std::get<0>(e);
@@ -441,10 +453,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   }
 
   if (_debug_visualizations) {
+    auto now = _clock->now();
     // Publish expansions for debug
     geometry_msgs::msg::PoseArray msg;
     geometry_msgs::msg::Pose msg_pose;
-    msg.header.stamp = _clock->now();
+    msg.header.stamp = now;
     msg.header.frame_id = _global_frame;
     for (auto & e : *expansions) {
       msg_pose.position.x = std::get<0>(e);
@@ -454,19 +467,20 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
     }
     _expansions_publisher->publish(msg);
 
-    // plot footprint path planned for debug
     if (_planned_footprints_publisher->get_subscription_count() > 0) {
+      // Clear all markers first
       auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      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));
+
+      // Publish smoothed footprints for debug
+      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()));
-      }
-
-      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);
+        marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
       }
       _planned_footprints_publisher->publish(std::move(marker_array));
     }
@@ -494,6 +508,27 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
     " milliseconds to smooth path." << std::endl;
 #endif
 
+  if (_debug_visualizations) {
+    if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
+      // Clear all markers first
+      auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      visualization_msgs::msg::Marker clear_all_marker;
+      clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
+      marker_array->markers.push_back(clear_all_marker);
+      _smoothed_footprints_publisher->publish(std::move(marker_array));
+
+      // Publish smoothed footprints for debug
+      marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
+      auto now = _clock->now();
+      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, now));
+      }
+      _smoothed_footprints_publisher->publish(std::move(marker_array));
+    }
+  }
+
   return plan;
 }