Validate Smac smoother footprint collisions

Signed-off-by: Alyss <u7288133@anu.edu.au>

Author: MobiusRoamer

nav2_smac_planner/include/nav2_smac_planner/smoother.hpp

@@ -23,6 +23,7 @@
 #include "nav2_util/geometry_utils.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "ompl/base/StateSpace.h"
+#include "geometry_msgs/msg/point.hpp"
 
 namespace nav2_smac_planner
 {
@@ -97,8 +98,9 @@ class Smoother
   bool smooth(
     nav_msgs::msg::Path & path,
     const nav2_costmap_2d::Costmap2D * costmap,
-    const double & max_time，
-    const nav2_costmap_2d::Footprint & footprint = nav2_costmap_2d::Footprint(),
+    const double & max_time,
+    const std::vector<geometry_msgs::msg::Point> & footprint =
+    std::vector<geometry_msgs::msg::Point>(),
     const bool & use_radius = true);
 
 protected:
@@ -115,7 +117,7 @@ class Smoother
     bool & reversing_segment,
     const nav2_costmap_2d::Costmap2D * costmap,
     const double & max_time,
-    const nav2_costmap_2d::Footprint & footprint,
+    const std::vector<geometry_msgs::msg::Point> & footprint,
     const bool & use_radius);
 
   /**

nav2_smac_planner/src/smoother.cpp

@@ -54,9 +54,8 @@ bool Smoother::smooth(
   nav_msgs::msg::Path & path,
   const nav2_costmap_2d::Costmap2D * costmap,
   const double & max_time,
-  const double & max_time,
-  const nav2_costmap_2d::Footprint & footprint = nav2_costmap_2d::Footprint(),
-  const bool & use_radius = true)
+  const std::vector<geometry_msgs::msg::Point> & footprint,
+  const bool & use_radius)
 {
   // by-pass path orientations approximation when skipping smac smoother
   if (max_its_ == 0) {
@@ -90,7 +89,8 @@ bool Smoother::smooth(
       const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose;
       const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose;
       bool local_success =
-        smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining, 
+        smoothImpl(
+        curr_path_segment, reversing_segment, costmap, time_remaining,
         footprint, use_radius);
       success = success && local_success;
 
@@ -115,9 +115,8 @@ bool Smoother::smoothImpl(
   nav_msgs::msg::Path & path,
   bool & reversing_segment,
   const nav2_costmap_2d::Costmap2D * costmap,
-  const double & max_time，
   const double & max_time,
-  const nav2_costmap_2d::Footprint & footprint,
+  const std::vector<geometry_msgs::msg::Point> & footprint,
   const bool & use_radius)
 {
   steady_clock::time_point a = steady_clock::now();
@@ -192,21 +191,21 @@ bool Smoother::smoothImpl(
         return false;
       }
     }
-    
+
     if (costmap && !use_radius) {
       nav2_util::updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_);
 
-      nav2_costmap_2d::FootprintCollisionChecker<const nav2_costmap_2d::Costmap2D *>
-      footprint_checker(costmap);
+      nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
+      footprint_checker(const_cast<nav2_costmap_2d::Costmap2D *>(costmap));
 
       for (const auto & pose : new_path.poses) {
         const double footprint_cost = footprint_checker.footprintCostAtPose(
-          pose.pose.position.x, 
-          pose.pose.position.y, 
-          f2f::getYaw(pose.pose.orientation), 
+          pose.pose.position.x,
+          pose.pose.position.y,
+          tf2::getYaw(pose.pose.orientation),
           footprint);
-        
-        if (footprint_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) {
+
+        if (footprint_cost > MAX_NON_OBSTACLE_COST && footprint_cost != UNKNOWN_COST) {
           RCLCPP_DEBUG(
             rclcpp::get_logger("SmacPlannerSmoother"),
             "Smoothing process resulted in an infeasible footprint collision. "

nav2_smac_planner/test/test_smoother.cpp

@@ -95,7 +95,7 @@ TEST(SmootherTest, test_full_smoother)
 
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
     std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, node);
-  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+  checker->setFootprint(std::vector<geometry_msgs::msg::Point>(), true, 0.0);
 
   auto dummy_cancel_checker = []() {
       return false;
@@ -201,9 +201,9 @@ TEST(SmootherTest, rejects_non_circular_footprint_collision)
   nav2_costmap_2d::Costmap2D costmap(
     200, 200, 0.1, 0.0, 0.0, nav2_costmap_2d::FREE_SPACE);
 
-  nav2_costmap_2d::Footprint footprint;
+  std::vector<geometry_msgs::msg::Point> footprint;
   for (const auto & coordinates : std::vector<std::pair<double, double>>{
-      {-0.6, -0.2}, {0.6, -0.2}, {0.6, 0.2}, {-0.6, 0.2}})
+    {-0.6, -0.2}, {0.6, -0.2}, {0.6, 0.2}, {-0.6, 0.2}})
   {
     geometry_msgs::msg::Point point;
     point.x = coordinates.first;