fix simple smoother failing during final approach (#4817)

* new test case for end of path approach

Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com>

* modify tests to match the more permissive smoother policy

Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com>

* implement steve's suggestions

Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com>

---------

Signed-off-by: rayferric <63957587+rayferric@users.noreply.github.com>

Author: rayferric

nav2_smoother/include/nav2_smoother/simple_smoother.hpp

@@ -92,9 +92,8 @@ class SimpleSmoother : public nav2_core::Smoother
    * @param reversing_segment Return if this is a reversing segment
    * @param costmap Pointer to minimal costmap
    * @param max_time Maximum time to compute, stop early if over limit
-   * @return If smoothing was successful
    */
-  bool smoothImpl(
+  void smoothImpl(
     nav_msgs::msg::Path & path,
     bool & reversing_segment,
     const nav2_costmap_2d::Costmap2D * costmap,

nav2_smoother/src/simple_smoother.cpp

@@ -65,8 +65,7 @@ bool SimpleSmoother::smooth(
   steady_clock::time_point start = steady_clock::now();
   double time_remaining = max_time.seconds();
 
-  bool success = true, reversing_segment;
-  unsigned int segments_smoothed = 0;
+  bool reversing_segment;
   nav_msgs::msg::Path curr_path_segment;
   curr_path_segment.header = path.header;
 
@@ -88,15 +87,9 @@ bool SimpleSmoother::smooth(
       time_remaining = max_time.seconds() - duration_cast<duration<double>>(now - start).count();
       refinement_ctr_ = 0;
 
-      bool segment_was_smoothed = smoothImpl(
-        curr_path_segment, reversing_segment, costmap.get(), time_remaining);
-
-      if (segment_was_smoothed) {
-        segments_smoothed++;
-      }
-
-      // Smooth path segment naively
-      success = success && segment_was_smoothed;
+      // Attempt to smooth the segment
+      // May throw SmootherTimedOut
+      smoothImpl(curr_path_segment, reversing_segment, costmap.get(), time_remaining);
 
       // Assemble the path changes to the main path
       std::copy(
@@ -106,14 +99,10 @@ bool SimpleSmoother::smooth(
     }
   }
 
-  if (segments_smoothed == 0) {
-    throw nav2_core::FailedToSmoothPath("No segments were smoothed");
-  }
-
-  return success;
+  return true;
 }
 
-bool SimpleSmoother::smoothImpl(
+void SimpleSmoother::smoothImpl(
   nav_msgs::msg::Path & path,
   bool & reversing_segment,
   const nav2_costmap_2d::Costmap2D * costmap,
@@ -142,7 +131,7 @@ bool SimpleSmoother::smoothImpl(
         "Number of iterations has exceeded limit of %i.", max_its_);
       path = last_path;
       updateApproximatePathOrientations(path, reversing_segment);
-      return false;
+      return;
     }
 
     // Make sure still have time left to process
@@ -188,7 +177,7 @@ bool SimpleSmoother::smoothImpl(
           "Returning the last path before the infeasibility was introduced.");
         path = last_path;
         updateApproximatePathOrientations(path, reversing_segment);
-        return false;
+        return;
       }
     }
 
@@ -204,7 +193,6 @@ bool SimpleSmoother::smoothImpl(
 
   updateApproximatePathOrientations(new_path, reversing_segment);
   path = new_path;
-  return true;
 }
 
 double SimpleSmoother::getFieldByDim(

nav2_smoother/test/test_simple_smoother.cpp

@@ -180,7 +180,7 @@ TEST(SmootherTest, test_simple_smoother)
   EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.607, 0.01);
   EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01);
 
-  // Test that collisions are rejected
+  // Test that collisions are disregarded
   nav_msgs::msg::Path collision_path;
   collision_path.poses.resize(11);
   collision_path.poses[0].pose.position.x = 0.0;
@@ -205,7 +205,7 @@ TEST(SmootherTest, test_simple_smoother)
   collision_path.poses[9].pose.position.y = 1.4;
   collision_path.poses[10].pose.position.x = 1.5;
   collision_path.poses[10].pose.position.y = 1.5;
-  EXPECT_THROW(smoother->smooth(collision_path, max_time), nav2_core::FailedToSmoothPath);
+  EXPECT_TRUE(smoother->smooth(collision_path, max_time));
 
   // test cusp / reversing segments
   nav_msgs::msg::Path reversing_path;
@@ -232,7 +232,7 @@ TEST(SmootherTest, test_simple_smoother)
   reversing_path.poses[9].pose.position.y = 0.1;
   reversing_path.poses[10].pose.position.x = 0.5;
   reversing_path.poses[10].pose.position.y = 0.0;
-  EXPECT_THROW(smoother->smooth(reversing_path, max_time), nav2_core::FailedToSmoothPath);
+  EXPECT_TRUE(smoother->smooth(reversing_path, max_time));
 
   // test rotate in place
   tf2::Quaternion quat1, quat2;
@@ -244,7 +244,18 @@ TEST(SmootherTest, test_simple_smoother)
   straight_irregular_path.poses[6].pose.position.x = 0.5;
   straight_irregular_path.poses[6].pose.position.y = 0.5;
   straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2);
-  EXPECT_THROW(smoother->smooth(straight_irregular_path, max_time), nav2_core::FailedToSmoothPath);
+  EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time));
+
+  // test approach
+  nav_msgs::msg::Path approach_path;
+  approach_path.poses.resize(3);
+  approach_path.poses[0].pose.position.x = 0.5;
+  approach_path.poses[0].pose.position.y = 0.0;
+  approach_path.poses[1].pose.position.x = 0.5;
+  approach_path.poses[1].pose.position.y = 0.1;
+  approach_path.poses[2].pose.position.x = 0.5;
+  approach_path.poses[2].pose.position.y = 0.2;
+  EXPECT_TRUE(smoother->smooth(approach_path, max_time));
 
   // test max iterations
   smoother->setMaxItsToInvalid();
@@ -272,5 +283,5 @@ TEST(SmootherTest, test_simple_smoother)
   max_its_path.poses[9].pose.position.y = 0.9;
   max_its_path.poses[10].pose.position.x = 0.5;
   max_its_path.poses[10].pose.position.y = 1.0;
-  EXPECT_THROW(smoother->smooth(max_its_path, max_time), nav2_core::FailedToSmoothPath);
+  EXPECT_TRUE(smoother->smooth(max_its_path, max_time));
 }