diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 3686f433376..1169c9d5ff9 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/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, diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 0b494d96c6e..5b4c2fff6e0 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/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>(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( diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index c8f7da8e871..6c845bfb6c6 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/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)); }