From 6ca216cc106a6b6a7b3bd1c2501a521c9098efc1 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Sun, 10 Aug 2025 17:42:33 +0300 Subject: [PATCH 1/2] Remove the smoother specific to smac and incorporate it to nav2_smoother Signed-off-by: CihatAltiparmak --- nav2_smac_planner/test/test_smoother.cpp | 22 ++ nav2_smoother/CMakeLists.txt | 10 +- .../include/nav2_smoother/simple_smoother.hpp | 7 +- .../include/nav2_smoother/smoother_utils.hpp | 266 +++++++++++++++++- nav2_smoother/package.xml | 1 + nav2_smoother/src/savitzky_golay_smoother.cpp | 2 +- nav2_smoother/src/simple_smoother.cpp | 47 +++- nav2_smoother/test/test_simple_smoother.cpp | 254 ++++++++++++++++- 8 files changed, 589 insertions(+), 20 deletions(-) diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 6c223176c23..295ce7e93a8 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -110,6 +110,9 @@ TEST(SmootherTest, test_full_smoother) nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), "###############"); + // Convert to world coordinates and get length to compare to smoothed length nav_msgs::msg::Path plan; plan.header.stamp = node->now(); @@ -131,6 +134,25 @@ TEST(SmootherTest, test_full_smoother) initial_length += hypot(path[i].x - x_m, path[i].y - y_m); x_m = path[i].x; y_m = path[i].y; + + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), + "smac_path.poses[%ld].pose.position.x = %f;", path.size() - i - 1, pose.pose.position.x); + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), + "smac_path.poses[%ld].pose.position.y = %f;", path.size() - i - 1, pose.pose.position.y); + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), + "smac_path.poses[%ld].pose.orientation.x = %f;", path.size() - i - 1, pose.pose.orientation.x); + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), + "smac_path.poses[%ld].pose.orientation.y = %f;", path.size() - i - 1, pose.pose.orientation.y); + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), + "smac_path.poses[%ld].pose.orientation.z = %f;", path.size() - i - 1, pose.pose.orientation.z); + RCLCPP_WARN( + rclcpp::get_logger("SmacPlannerSmoother"), + "smac_path.poses[%ld].pose.orientation.w = %f;", path.size() - i - 1, pose.pose.orientation.w); } // Check that we accurately detect that this path has a reversing segment diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 5831d10189c..7e0e434a04f 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) +find_package(ompl REQUIRED) nav2_package() @@ -65,7 +66,8 @@ add_library(simple_smoother SHARED target_include_directories(simple_smoother PUBLIC "$" - "$") + "$" + ${OMPL_INCLUDE_DIRS}) target_link_libraries(simple_smoother PUBLIC angles::angles nav2_core::nav2_core @@ -78,6 +80,7 @@ target_link_libraries(simple_smoother PUBLIC tf2::tf2 tf2_ros::tf2_ros nav2_ros_common::nav2_ros_common + ${OMPL_LIBRARIES} ) target_link_libraries(simple_smoother PRIVATE pluginlib::pluginlib @@ -90,7 +93,8 @@ add_library(savitzky_golay_smoother SHARED target_include_directories(savitzky_golay_smoother PUBLIC "$" - "$") + "$" + ${OMPL_INCLUDE_DIRS}) target_link_libraries(savitzky_golay_smoother PUBLIC angles::angles nav2_core::nav2_core @@ -103,6 +107,7 @@ target_link_libraries(savitzky_golay_smoother PUBLIC tf2::tf2 nav2_ros_common::nav2_ros_common tf2_ros::tf2_ros + ${OMPL_LIBRARIES} ) target_link_libraries(savitzky_golay_smoother PRIVATE pluginlib::pluginlib @@ -157,6 +162,7 @@ ament_export_dependencies( rclcpp_lifecycle tf2 tf2_ros + ompl ) ament_export_targets(${library_name}) ament_package() diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index dfaeba953aa..0afa710856c 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -94,7 +94,7 @@ class SimpleSmoother : public nav2_core::Smoother * @param costmap Pointer to minimal costmap * @param max_time Maximum time to compute, stop early if over limit */ - void smoothImpl( + bool smoothImpl( nav_msgs::msg::Path & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, @@ -120,9 +120,10 @@ class SimpleSmoother : public nav2_core::Smoother geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - double tolerance_, data_w_, smooth_w_; + double tolerance_, data_w_, smooth_w_, min_turning_radius_; int max_its_, refinement_ctr_, refinement_num_; - bool do_refinement_, enforce_path_inversion_; + bool is_holonomic_, do_refinement_, enforce_path_inversion_; + ompl::base::StateSpacePtr state_space_; std::shared_ptr costmap_sub_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; }; diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp index 500dd7a9351..e7fbc81875e 100644 --- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -31,10 +31,16 @@ #include "nav_msgs/msg/path.hpp" #include "angles/angles.h" #include "tf2/utils.hpp" +#include "ompl/base/StateSpace.h" +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" namespace smoother_utils { +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT + /** * @class nav2_smoother::PathSegment * @brief A segment of a path in start/end indices @@ -45,6 +51,38 @@ struct PathSegment unsigned int end; }; +/** + * @struct nav2_smac_planner::BoundaryPoints + * @brief Set of boundary condition points from expansion + */ +struct BoundaryPoints +{ + /** + * @brief A constructor for BoundaryPoints + */ + BoundaryPoints(double & x_in, double & y_in, double & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + double x; + double y; + double theta; +}; + +/** + * @struct nav2_smac_planner::BoundaryExpansion + * @brief Boundary expansion state + */ +struct BoundaryExpansion +{ + double path_end_idx{0.0}; + double expansion_path_length{0.0}; + double original_path_length{0.0}; + std::vector pts; + bool in_collision{false}; +}; + +typedef std::vector BoundaryExpansions; typedef std::vector::iterator PathIterator; typedef std::vector::reverse_iterator ReversePathIterator; @@ -93,7 +131,8 @@ inline std::vector findDirectionalPathSegments( inline void updateApproximatePathOrientations( nav_msgs::msg::Path & path, - bool & reversing_segment) + bool & reversing_segment, + bool is_holonomic) { double dx, dy, theta, pt_yaw; reversing_segment = false; @@ -103,7 +142,7 @@ inline void updateApproximatePathOrientations( dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; theta = atan2(dy, dx); pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + if (!is_holonomic && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { reversing_segment = true; } @@ -127,6 +166,229 @@ inline void updateApproximatePathOrientations( } } +unsigned int findShortestBoundaryExpansionIdx( + const BoundaryExpansions & boundary_expansions) +{ + // Check which is valid with the minimum integrated length such that + // shorter end-points away that are infeasible to achieve without + // a loop-de-loop are punished + double min_length = 1e9; + int shortest_boundary_expansion_idx = 1e9; + for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { + if (boundary_expansions[idx].expansion_path_length0.0 && + boundary_expansions[idx].expansion_path_length > 0.0) + { + min_length = boundary_expansions[idx].expansion_path_length; + shortest_boundary_expansion_idx = idx; + } + } + + return shortest_boundary_expansion_idx; +} + +void findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const ompl::base::StateSpacePtr & state_space, + const nav2_costmap_2d::Costmap2D * costmap) +{ + static ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space); + + from[0] = start.position.x; + from[1] = start.position.y; + from[2] = tf2::getYaw(start.orientation); + to[0] = end.position.x; + to[1] = end.position.y; + to[2] = tf2::getYaw(end.orientation); + + double d = state_space->distance(from(), to()); + // If this path is too long compared to the original, then this is probably + // a loop-de-loop, treat as invalid as to not deviate too far from the original path. + // 2.0 selected from prinicipled choice of boundary test points + // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be + // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. + // For all but the last backup test point, a loop would be approximately + // 2x greater than any of the selections. + if (d > 2.0 * expansion.original_path_length) { + return; + } + + std::vector reals; + double theta(0.0), x(0.0), y(0.0); + double x_m = start.position.x; + double y_m = start.position.y; + + // Get intermediary poses + for (double i = 0; i <= expansion.path_end_idx; i++) { + state_space->interpolate(from(), to(), i / expansion.path_end_idx, s()); + reals = s.reals(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + x = reals[0]; + y = reals[1]; + + // Check for collision + unsigned int mx, my; + costmap->worldToMap(x, y, mx, my); + if (static_cast(costmap->getCost(mx, + my)) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + expansion.in_collision = true; + } + + // Integrate path length + expansion.expansion_path_length += hypot(x - x_m, y - y_m); + x_m = x; + y_m = y; + + // Store point + expansion.pts.emplace_back(x, y, theta); + } +} + +template +BoundaryExpansions generateBoundaryExpansionPoints( + IteratorT start, IteratorT end, + const double minimum_turning_radius) +{ + std::vector distances = { + minimum_turning_radius, // Radius + 2.0 * minimum_turning_radius, // Diameter + M_PI * minimum_turning_radius, // 50% Circumference + 2.0 * M_PI * minimum_turning_radius // Circumference + }; + + BoundaryExpansions boundary_expansions; + boundary_expansions.resize(distances.size()); + double curr_dist = 0.0; + double x_last = start->pose.position.x; + double y_last = start->pose.position.y; + geometry_msgs::msg::Point pt; + unsigned int curr_dist_idx = 0; + + for (IteratorT iter = start; iter != end; iter++) { + pt = iter->pose.position; + curr_dist += hypot(pt.x - x_last, pt.y - y_last); + x_last = pt.x; + y_last = pt.y; + + if (curr_dist >= distances[curr_dist_idx]) { + boundary_expansions[curr_dist_idx].path_end_idx = iter - start; + boundary_expansions[curr_dist_idx].original_path_length = curr_dist; + curr_dist_idx++; + } + + if (curr_dist_idx == boundary_expansions.size()) { + break; + } + } + + return boundary_expansions; +} + +void enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const ompl::base::StateSpacePtr & state_space, + const double minimum_turning_radius, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end(), + minimum_turning_radius); + + // Generate the motion model and metadata from start -> test points + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + + if (!reversing_segment) { + findBoundaryExpansion( + start_pose, path.poses[expansion.path_end_idx].pose, expansion, + state_space, costmap); + } else { + findBoundaryExpansion( + path.poses[expansion.path_end_idx].pose, start_pose, expansion, + state_space, costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[i].pose.position.x = best_expansion.pts[i].x; + path.poses[i].pose.position.y = best_expansion.pts[i].y; + path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); + } +} + +void enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const ompl::base::StateSpacePtr & state_space, + const double minimum_turning_radius, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend(), + minimum_turning_radius); + + // Generate the motion model and metadata from start -> test points + unsigned int expansion_starting_idx; + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; + if (!reversing_segment) { + findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, + state_space, costmap); + } else { + findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, + state_space, costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; + path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; + path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( + best_expansion.pts[i].theta); + } +} + } // namespace smoother_utils #endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 339bdeb5586..d256219755f 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -24,6 +24,7 @@ tf2 tf2_ros nav2_ros_common + ompl ament_cmake_gtest ament_index_cpp diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index af23cf30995..740880300fe 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -166,7 +166,7 @@ bool SavitzkyGolaySmoother::smoothImpl( } } - updateApproximatePathOrientations(path, reversing_segment); + updateApproximatePathOrientations(path, reversing_segment, false); return true; } diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index db9f323e8a1..518edd5a8ed 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -43,6 +43,10 @@ void SimpleSmoother::configure( node, name + ".w_data", rclcpp::ParameterValue(0.2)); declare_parameter_if_not_declared( node, name + ".w_smooth", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, name + ".is_holonomic", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); declare_parameter_if_not_declared( node, name + ".do_refinement", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -54,9 +58,15 @@ void SimpleSmoother::configure( node->get_parameter(name + ".max_its", max_its_); node->get_parameter(name + ".w_data", data_w_); node->get_parameter(name + ".w_smooth", smooth_w_); + node->get_parameter(name + ".is_holonomic", is_holonomic_); + node->get_parameter(name + ".minimum_turning_radius", min_turning_radius_); node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); + + if (!is_holonomic_) { + state_space_ = std::make_unique(min_turning_radius_); + } } bool SimpleSmoother::smooth( @@ -80,6 +90,7 @@ bool SimpleSmoother::smooth( std::lock_guard lock(*(costmap->getMutex())); + bool success = true; for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { // Populate path segment @@ -94,9 +105,23 @@ bool SimpleSmoother::smooth( time_remaining = max_time.seconds() - duration_cast>(now - start).count(); refinement_ctr_ = 0; + // Smooth path segment naively + 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; + // Attempt to smooth the segment // May throw SmootherTimedOut - smoothImpl(curr_path_segment, reversing_segment, costmap.get(), time_remaining); + bool local_success = smoothImpl(curr_path_segment, reversing_segment, costmap.get(), + time_remaining); + success = success && local_success; + + // Enforce boundary conditions + if (!is_holonomic_ && local_success) { + enforceStartBoundaryConditions(start_pose, curr_path_segment, state_space_, + min_turning_radius_, costmap.get(), reversing_segment); + enforceEndBoundaryConditions(goal_pose, curr_path_segment, state_space_, + min_turning_radius_, costmap.get(), reversing_segment); + } // Assemble the path changes to the main path std::copy( @@ -106,10 +131,10 @@ bool SimpleSmoother::smooth( } } - return true; + return success; } -void SimpleSmoother::smoothImpl( +bool SimpleSmoother::smoothImpl( nav_msgs::msg::Path & path, bool & reversing_segment, const nav2_costmap_2d::Costmap2D * costmap, @@ -137,8 +162,8 @@ void SimpleSmoother::smoothImpl( logger_, "Number of iterations has exceeded limit of %i.", max_its_); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); - return; + updateApproximatePathOrientations(path, reversing_segment, is_holonomic_); + return false; } // Make sure still have time left to process @@ -149,7 +174,7 @@ void SimpleSmoother::smoothImpl( logger_, "Smoothing time exceeded allowed duration of %0.2f.", max_time); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); + updateApproximatePathOrientations(path, reversing_segment, is_holonomic_); throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration"); } @@ -178,13 +203,13 @@ void SimpleSmoother::smoothImpl( } if (cost > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION) { - RCLCPP_DEBUG( + RCLCPP_WARN( rclcpp::get_logger("SmacPlannerSmoother"), "Smoothing process resulted in an infeasible collision. " "Returning the last path before the infeasibility was introduced."); path = last_path; - updateApproximatePathOrientations(path, reversing_segment); - return; + updateApproximatePathOrientations(path, reversing_segment, is_holonomic_); + return false; } } @@ -198,8 +223,10 @@ void SimpleSmoother::smoothImpl( smoothImpl(new_path, reversing_segment, costmap, max_time); } - updateApproximatePathOrientations(new_path, reversing_segment); + updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_); 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 311b9838276..376c28da733 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -45,6 +45,20 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother { max_its_ = 0; } + + double getPathLength(const nav_msgs::msg::Path & path) { + if (path.poses.size() == 0) { + return 0.0; + } + + double path_length = 0.0; + for (size_t i = 0; i < path.poses.size() - 1; ++i) { + double dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + double dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + path_length += std::hypot(dx, dy); + } + return path_length; + } }; TEST(SmootherTest, test_simple_smoother) @@ -196,7 +210,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_TRUE(smoother->smooth(collision_path, max_time)); + EXPECT_FALSE(smoother->smooth(collision_path, max_time)); // test cusp / reversing segments nav_msgs::msg::Path reversing_path; @@ -248,6 +262,242 @@ TEST(SmootherTest, test_simple_smoother) approach_path.poses[2].pose.position.y = 0.2; EXPECT_TRUE(smoother->smooth(approach_path, max_time)); + nav_msgs::msg::Path smac_path; + smac_path.poses[0].pose.position.x = 0.250000; + smac_path.poses[0].pose.position.y = 0.250000; + smac_path.poses[0].pose.orientation.x = 0.000000; + smac_path.poses[0].pose.orientation.y = 0.000000; + smac_path.poses[0].pose.orientation.z = 0.000000; + smac_path.poses[0].pose.orientation.w = 1.000000; + smac_path.poses[1].pose.position.x = 0.353528; + smac_path.poses[1].pose.position.y = 0.263630; + smac_path.poses[1].pose.orientation.x = 0.000000; + smac_path.poses[1].pose.orientation.y = 0.000000; + smac_path.poses[1].pose.orientation.z = 0.130526; + smac_path.poses[1].pose.orientation.w = 0.991445; + smac_path.poses[2].pose.position.x = 0.450000; + smac_path.poses[2].pose.position.y = 0.303590; + smac_path.poses[2].pose.orientation.x = 0.000000; + smac_path.poses[2].pose.orientation.y = 0.000000; + smac_path.poses[2].pose.orientation.z = 0.258819; + smac_path.poses[2].pose.orientation.w = 0.965926; + smac_path.poses[3].pose.position.x = 0.540431; + smac_path.poses[3].pose.position.y = 0.355800; + smac_path.poses[3].pose.orientation.x = 0.000000; + smac_path.poses[3].pose.orientation.y = 0.000000; + smac_path.poses[3].pose.orientation.z = 0.258819; + smac_path.poses[3].pose.orientation.w = 0.965926; + smac_path.poses[4].pose.position.x = 0.630862; + smac_path.poses[4].pose.position.y = 0.408011; + smac_path.poses[4].pose.orientation.x = 0.000000; + smac_path.poses[4].pose.orientation.y = 0.000000; + smac_path.poses[4].pose.orientation.z = 0.258819; + smac_path.poses[4].pose.orientation.w = 0.965926; + smac_path.poses[5].pose.position.x = 0.721294; + smac_path.poses[5].pose.position.y = 0.460221; + smac_path.poses[5].pose.orientation.x = 0.000000; + smac_path.poses[5].pose.orientation.y = 0.000000; + smac_path.poses[5].pose.orientation.z = 0.258819; + smac_path.poses[5].pose.orientation.w = 0.965926; + smac_path.poses[6].pose.position.x = 0.811725; + smac_path.poses[6].pose.position.y = 0.512432; + smac_path.poses[6].pose.orientation.x = 0.000000; + smac_path.poses[6].pose.orientation.y = 0.000000; + smac_path.poses[6].pose.orientation.z = 0.258819; + smac_path.poses[6].pose.orientation.w = 0.965926; + smac_path.poses[7].pose.position.x = 0.902156; + smac_path.poses[7].pose.position.y = 0.564642; + smac_path.poses[7].pose.orientation.x = 0.000000; + smac_path.poses[7].pose.orientation.y = 0.000000; + smac_path.poses[7].pose.orientation.z = 0.258819; + smac_path.poses[7].pose.orientation.w = 0.965926; + smac_path.poses[8].pose.position.x = 0.992587; + smac_path.poses[8].pose.position.y = 0.616853; + smac_path.poses[8].pose.orientation.x = 0.000000; + smac_path.poses[8].pose.orientation.y = 0.000000; + smac_path.poses[8].pose.orientation.z = 0.258819; + smac_path.poses[8].pose.orientation.w = 0.965926; + smac_path.poses[9].pose.position.x = 1.083018; + smac_path.poses[9].pose.position.y = 0.669063; + smac_path.poses[9].pose.orientation.x = 0.000000; + smac_path.poses[9].pose.orientation.y = 0.000000; + smac_path.poses[9].pose.orientation.z = 0.258819; + smac_path.poses[9].pose.orientation.w = 0.965926; + smac_path.poses[10].pose.position.x = 1.173450; + smac_path.poses[10].pose.position.y = 0.721274; + smac_path.poses[10].pose.orientation.x = 0.000000; + smac_path.poses[10].pose.orientation.y = 0.000000; + smac_path.poses[10].pose.orientation.z = 0.258819; + smac_path.poses[10].pose.orientation.w = 0.965926; + smac_path.poses[11].pose.position.x = 1.263881; + smac_path.poses[11].pose.position.y = 0.773484; + smac_path.poses[11].pose.orientation.x = 0.000000; + smac_path.poses[11].pose.orientation.y = 0.000000; + smac_path.poses[11].pose.orientation.z = 0.258819; + smac_path.poses[11].pose.orientation.w = 0.965926; + smac_path.poses[12].pose.position.x = 1.354312; + smac_path.poses[12].pose.position.y = 0.825695; + smac_path.poses[12].pose.orientation.x = 0.000000; + smac_path.poses[12].pose.orientation.y = 0.000000; + smac_path.poses[12].pose.orientation.z = 0.258819; + smac_path.poses[12].pose.orientation.w = 0.965926; + smac_path.poses[13].pose.position.x = 1.437155; + smac_path.poses[13].pose.position.y = 0.889262; + smac_path.poses[13].pose.orientation.x = 0.000000; + smac_path.poses[13].pose.orientation.y = 0.000000; + smac_path.poses[13].pose.orientation.z = 0.382683; + smac_path.poses[13].pose.orientation.w = 0.923880; + smac_path.poses[14].pose.position.x = 1.510992; + smac_path.poses[14].pose.position.y = 0.963099; + smac_path.poses[14].pose.orientation.x = 0.000000; + smac_path.poses[14].pose.orientation.y = 0.000000; + smac_path.poses[14].pose.orientation.z = 0.382683; + smac_path.poses[14].pose.orientation.w = 0.923880; + smac_path.poses[15].pose.position.x = 1.584828; + smac_path.poses[15].pose.position.y = 1.036936; + smac_path.poses[15].pose.orientation.x = 0.000000; + smac_path.poses[15].pose.orientation.y = 0.000000; + smac_path.poses[15].pose.orientation.z = 0.382683; + smac_path.poses[15].pose.orientation.w = 0.923880; + smac_path.poses[16].pose.position.x = 1.658665; + smac_path.poses[16].pose.position.y = 1.110772; + smac_path.poses[16].pose.orientation.x = 0.000000; + smac_path.poses[16].pose.orientation.y = 0.000000; + smac_path.poses[16].pose.orientation.z = 0.382683; + smac_path.poses[16].pose.orientation.w = 0.923880; + smac_path.poses[17].pose.position.x = 1.732502; + smac_path.poses[17].pose.position.y = 1.184609; + smac_path.poses[17].pose.orientation.x = 0.000000; + smac_path.poses[17].pose.orientation.y = 0.000000; + smac_path.poses[17].pose.orientation.z = 0.382683; + smac_path.poses[17].pose.orientation.w = 0.923880; + smac_path.poses[18].pose.position.x = 1.806339; + smac_path.poses[18].pose.position.y = 1.258446; + smac_path.poses[18].pose.orientation.x = 0.000000; + smac_path.poses[18].pose.orientation.y = 0.000000; + smac_path.poses[18].pose.orientation.z = 0.382683; + smac_path.poses[18].pose.orientation.w = 0.923880; + smac_path.poses[19].pose.position.x = 1.880175; + smac_path.poses[19].pose.position.y = 1.332283; + smac_path.poses[19].pose.orientation.x = 0.000000; + smac_path.poses[19].pose.orientation.y = 0.000000; + smac_path.poses[19].pose.orientation.z = 0.382683; + smac_path.poses[19].pose.orientation.w = 0.923880; + smac_path.poses[20].pose.position.x = 1.943743; + smac_path.poses[20].pose.position.y = 1.415126; + smac_path.poses[20].pose.orientation.x = 0.000000; + smac_path.poses[20].pose.orientation.y = 0.000000; + smac_path.poses[20].pose.orientation.z = 0.500000; + smac_path.poses[20].pose.orientation.w = 0.866025; + smac_path.poses[21].pose.position.x = 1.995953; + smac_path.poses[21].pose.position.y = 1.505557; + smac_path.poses[21].pose.orientation.x = 0.000000; + smac_path.poses[21].pose.orientation.y = 0.000000; + smac_path.poses[21].pose.orientation.z = 0.500000; + smac_path.poses[21].pose.orientation.w = 0.866025; + smac_path.poses[22].pose.position.x = 2.035913; + smac_path.poses[22].pose.position.y = 1.602029; + smac_path.poses[22].pose.orientation.x = 0.000000; + smac_path.poses[22].pose.orientation.y = 0.000000; + smac_path.poses[22].pose.orientation.z = 0.608761; + smac_path.poses[22].pose.orientation.w = 0.793353; + smac_path.poses[23].pose.position.x = 2.062939; + smac_path.poses[23].pose.position.y = 1.702892; + smac_path.poses[23].pose.orientation.x = 0.000000; + smac_path.poses[23].pose.orientation.y = 0.000000; + smac_path.poses[23].pose.orientation.z = 0.608761; + smac_path.poses[23].pose.orientation.w = 0.793353; + smac_path.poses[24].pose.position.x = 2.088483; + smac_path.poses[24].pose.position.y = 1.772004; + smac_path.poses[24].pose.orientation.x = 0.000000; + smac_path.poses[24].pose.orientation.y = 0.000000; + smac_path.poses[24].pose.orientation.z = 0.500000; + smac_path.poses[24].pose.orientation.w = 0.866025; + smac_path.poses[25].pose.position.x = 2.123730; + smac_path.poses[25].pose.position.y = 1.836797; + smac_path.poses[25].pose.orientation.x = 0.000000; + smac_path.poses[25].pose.orientation.y = 0.000000; + smac_path.poses[25].pose.orientation.z = 0.500000; + smac_path.poses[25].pose.orientation.w = 0.866025; + smac_path.poses[26].pose.position.x = 2.150105; + smac_path.poses[26].pose.position.y = 1.905596; + smac_path.poses[26].pose.orientation.x = 0.000000; + smac_path.poses[26].pose.orientation.y = 0.000000; + smac_path.poses[26].pose.orientation.z = 0.573576; + smac_path.poses[26].pose.orientation.w = 0.819152; + smac_path.poses[27].pose.position.x = 2.163413; + smac_path.poses[27].pose.position.y = 1.978065; + smac_path.poses[27].pose.orientation.x = 0.000000; + smac_path.poses[27].pose.orientation.y = 0.000000; + smac_path.poses[27].pose.orientation.z = 0.642788; + smac_path.poses[27].pose.orientation.w = 0.766044; + smac_path.poses[28].pose.position.x = 2.163204; + smac_path.poses[28].pose.position.y = 2.051746; + smac_path.poses[28].pose.orientation.x = 0.000000; + smac_path.poses[28].pose.orientation.y = 0.000000; + smac_path.poses[28].pose.orientation.z = 0.737277; + smac_path.poses[28].pose.orientation.w = 0.675590; + smac_path.poses[29].pose.position.x = 2.149483; + smac_path.poses[29].pose.position.y = 2.124139; + smac_path.poses[29].pose.orientation.x = 0.000000; + smac_path.poses[29].pose.orientation.y = 0.000000; + smac_path.poses[29].pose.orientation.z = 0.793353; + smac_path.poses[29].pose.orientation.w = 0.608761; + smac_path.poses[30].pose.position.x = 2.122717; + smac_path.poses[30].pose.position.y = 2.192787; + smac_path.poses[30].pose.orientation.x = 0.000000; + smac_path.poses[30].pose.orientation.y = 0.000000; + smac_path.poses[30].pose.orientation.z = 0.843391; + smac_path.poses[30].pose.orientation.w = 0.537300; + smac_path.poses[31].pose.position.x = 2.083813; + smac_path.poses[31].pose.position.y = 2.255361; + smac_path.poses[31].pose.orientation.x = 0.000000; + smac_path.poses[31].pose.orientation.y = 0.000000; + smac_path.poses[31].pose.orientation.z = 0.887011; + smac_path.poses[31].pose.orientation.w = 0.461749; + smac_path.poses[32].pose.position.x = 2.034093; + smac_path.poses[32].pose.position.y = 2.309737; + smac_path.poses[32].pose.orientation.x = 0.000000; + smac_path.poses[32].pose.orientation.y = 0.000000; + smac_path.poses[32].pose.orientation.z = 0.923880; + smac_path.poses[32].pose.orientation.w = 0.382683; + smac_path.poses[33].pose.position.x = 2.039769; + smac_path.poses[33].pose.position.y = 2.309702; + smac_path.poses[33].pose.orientation.x = 0.000000; + smac_path.poses[33].pose.orientation.y = 0.000000; + smac_path.poses[33].pose.orientation.z = 0.953717; + smac_path.poses[33].pose.orientation.w = 0.300706; + smac_path.poses[34].pose.position.x = 2.105753; + smac_path.poses[34].pose.position.y = 2.276914; + smac_path.poses[34].pose.orientation.x = 0.000000; + smac_path.poses[34].pose.orientation.y = 0.000000; + smac_path.poses[34].pose.orientation.z = 0.976296; + smac_path.poses[34].pose.orientation.w = 0.216440; + smac_path.poses[35].pose.position.x = 2.176632; + smac_path.poses[35].pose.position.y = 2.256786; + smac_path.poses[35].pose.orientation.x = 0.000000; + smac_path.poses[35].pose.orientation.y = 0.000000; + smac_path.poses[35].pose.orientation.z = 0.991445; + smac_path.poses[35].pose.orientation.w = 0.130526; + smac_path.poses[36].pose.position.x = 2.250000; + smac_path.poses[36].pose.position.y = 2.250000; + smac_path.poses[36].pose.orientation.x = 0.000000; + smac_path.poses[36].pose.orientation.y = -0.000000; + smac_path.poses[36].pose.orientation.z = 1.000000; + smac_path.poses[36].pose.orientation.w = -0.000000; + + // Check that we accurately detect that this path has a reversing segment + auto path_segs = smoother->findDirectionalPathSegmentsWrapper(smac_path); + EXPECT_TRUE(path_segs.size() == 2u || path_segs.size() == 3u); + + // Test smoother, should succeed with same number of points + // and shorter overall length, while still being collision free. + double initial_length = smoother->getPathLength(smac_path); + auto path_size_in = smac_path.poses.size(); + EXPECT_TRUE(smoother->smooth(smac_path, max_time)); + EXPECT_EQ(smac_path.poses.size(), path_size_in); // Should have same number of poses + EXPECT_LT(smoother->getPathLength(smac_path), initial_length); // Should be shorter + // test max iterations smoother->setMaxItsToInvalid(); nav_msgs::msg::Path max_its_path; @@ -274,7 +524,7 @@ 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_TRUE(smoother->smooth(max_its_path, max_time)); + EXPECT_FALSE(smoother->smooth(max_its_path, max_time)); } int main(int argc, char **argv) From 075db597991cebe6c59c311e9e71941c0763cf80 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 11 Aug 2025 01:28:13 +0300 Subject: [PATCH 2/2] Remove smoother relavant things from smac and fix out-of-bounds in test_simple_smoother Signed-off-by: CihatAltiparmak --- nav2_smac_planner/CMakeLists.txt | 1 - .../nav2_smac_planner/smac_planner_2d.hpp | 2 - .../nav2_smac_planner/smac_planner_hybrid.hpp | 5 - .../smac_planner_lattice.hpp | 5 - .../include/nav2_smac_planner/smoother.hpp | 235 -------- nav2_smac_planner/src/smac_planner_2d.cpp | 38 -- nav2_smac_planner/src/smac_planner_hybrid.cpp | 103 +--- .../src/smac_planner_lattice.cpp | 103 +--- nav2_smac_planner/src/smoother.cpp | 520 ------------------ nav2_smac_planner/test/CMakeLists.txt | 15 - nav2_smac_planner/test/test_smac_2d.cpp | 2 - nav2_smac_planner/test/test_smac_hybrid.cpp | 2 - nav2_smac_planner/test/test_smac_lattice.cpp | 1 - nav2_smac_planner/test/test_smoother.cpp | 227 -------- nav2_smoother/src/simple_smoother.cpp | 8 +- nav2_smoother/test/test_simple_smoother.cpp | 78 +-- 16 files changed, 55 insertions(+), 1290 deletions(-) delete mode 100644 nav2_smac_planner/include/nav2_smac_planner/smoother.hpp delete mode 100644 nav2_smac_planner/src/smoother.cpp delete mode 100644 nav2_smac_planner/test/test_smoother.cpp diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 051818f7c9a..b20f67689e7 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -42,7 +42,6 @@ add_library(${library_name}_common SHARED src/node_basic.cpp src/node_hybrid.cpp src/node_lattice.cpp - src/smoother.cpp ) # Add GenerateExportHeader support for symbol visibility, as we are using # static members we need to explicitly export them on Windows, as diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index a5ac6743f42..da30ee0e599 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -21,7 +21,6 @@ #include #include "nav2_smac_planner/a_star.hpp" -#include "nav2_smac_planner/smoother.hpp" #include "nav2_smac_planner/utils.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -100,7 +99,6 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner std::unique_ptr> _a_star; GridCollisionChecker _collision_checker; - std::unique_ptr _smoother; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; rclcpp::Clock::SharedPtr _clock; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 37136af087e..6ff11112bb9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -20,7 +20,6 @@ #include #include "nav2_smac_planner/a_star.hpp" -#include "nav2_smac_planner/smoother.hpp" #include "nav2_smac_planner/utils.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -99,7 +98,6 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner std::unique_ptr> _a_star; GridCollisionChecker _collision_checker; - std::unique_ptr _smoother; rclcpp::Clock::SharedPtr _clock; rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; nav2_costmap_2d::Costmap2D * _costmap; @@ -125,11 +123,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner MotionModel _motion_model; GoalHeadingMode _goal_heading_mode; int _coarse_search_resolution; - nav2::Publisher::SharedPtr _raw_plan_publisher; nav2::Publisher::SharedPtr _planned_footprints_publisher; - nav2::Publisher::SharedPtr - _smoothed_footprints_publisher; nav2::Publisher::SharedPtr _expansions_publisher; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 856734f5221..0d1d9efefc2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -20,7 +20,6 @@ #include #include "nav2_smac_planner/a_star.hpp" -#include "nav2_smac_planner/smoother.hpp" #include "nav2_smac_planner/utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" @@ -98,7 +97,6 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner std::unique_ptr> _a_star; GridCollisionChecker _collision_checker; - std::unique_ptr _smoother; rclcpp::Clock::SharedPtr _clock; rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerLattice")}; nav2_costmap_2d::Costmap2D * _costmap; @@ -112,14 +110,11 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner int _max_on_approach_iterations; int _terminal_checking_interval; float _tolerance; - nav2::Publisher::SharedPtr _raw_plan_publisher; double _max_planning_time; double _lookup_table_size; bool _debug_visualizations; nav2::Publisher::SharedPtr _planned_footprints_publisher; - nav2::Publisher::SharedPtr - _smoothed_footprints_publisher; nav2::Publisher::SharedPtr _expansions_publisher; GoalHeadingMode _goal_heading_mode; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp deleted file mode 100644 index 00503d4d293..00000000000 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ /dev/null @@ -1,235 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_ -#define NAV2_SMAC_PLANNER__SMOOTHER_HPP_ - -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/constants.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav_msgs/msg/path.hpp" -#include "ompl/base/StateSpace.h" - -namespace nav2_smac_planner -{ - -/** - * @class nav2_smac_planner::PathSegment - * @brief A segment of a path in start/end indices - */ -struct PathSegment -{ - unsigned int start; - unsigned int end; -}; - -/** - * @struct nav2_smac_planner::BoundaryPoints - * @brief Set of boundary condition points from expansion - */ -struct BoundaryPoints -{ - /** - * @brief A constructor for BoundaryPoints - */ - BoundaryPoints(double & x_in, double & y_in, double & theta_in) - : x(x_in), y(y_in), theta(theta_in) - {} - - double x; - double y; - double theta; -}; - -/** - * @struct nav2_smac_planner::BoundaryExpansion - * @brief Boundary expansion state - */ -struct BoundaryExpansion -{ - double path_end_idx{0.0}; - double expansion_path_length{0.0}; - double original_path_length{0.0}; - std::vector pts; - bool in_collision{false}; -}; - -typedef std::vector BoundaryExpansions; -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - -/** - * @class nav2_smac_planner::Smoother - * @brief A path smoother implementation - */ -class Smoother -{ -public: - /** - * @brief A constructor for nav2_smac_planner::Smoother - */ - explicit Smoother(const SmootherParams & params); - - /** - * @brief A destructor for nav2_smac_planner::Smoother - */ - ~Smoother() {} - - /** - * @brief Initialization of the smoother - * @param min_turning_radius Minimum turning radius (m) - * @param motion_model Motion model type - */ - void initialize( - const double & min_turning_radius); - - /** - * @brief Smoother API method - * @param path Reference to path - * @param costmap Pointer to minimal costmap - * @param max_time Maximum time to compute, stop early if over limit - * @return If smoothing was successful - */ - bool smooth( - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time); - -protected: - /** - * @brief Smoother method - does the smoothing on a segment - * @param path Reference to path - * @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( - nav_msgs::msg::Path & path, - bool & reversing_segment, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time); - - /** - * @brief Get the field value for a given dimension - * @param msg Current pose to sample - * @param dim Dimension ID of interest - * @return dim value - */ - inline double getFieldByDim( - const geometry_msgs::msg::PoseStamped & msg, - const unsigned int & dim); - - /** - * @brief Set the field value for a given dimension - * @param msg Current pose to sample - * @param dim Dimension ID of interest - * @param value to set the dimension to for the pose - */ - inline void setFieldByDim( - geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, - const double & value); - - /** - * @brief Finds the starting and end indices of path segments where - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param path Path in which to look for cusps - * @return Set of index pairs for each segment of the path in a given direction - */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); - - /** - * @brief Enforced minimum curvature boundary conditions on plan output - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param start_pose Start pose of the feasible path to maintain - * @param path Path to modify for curvature constraints on start / end of path - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void enforceStartBoundaryConditions( - const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment); - - /** - * @brief Enforced minimum curvature boundary conditions on plan output - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param end_pose End pose of the feasible path to maintain - * @param path Path to modify for curvature constraints on start / end of path - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void enforceEndBoundaryConditions( - const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment); - - /** - * @brief Given a set of boundary expansion, find the one which is shortest - * such that it is least likely to contain a loop-de-loop when working with - * close-by primitive markers. Instead, select a further away marker which - * generates a shorter ` - * @param boundary_expansions Set of boundary expansions - * @return Idx of the shorest boundary expansion option - */ - unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions); - - /** - * @brief Populate a motion model expansion from start->end into expansion - * @param start Start pose of the feasible path to maintain - * @param end End pose of the feasible path to maintain - * @param expansion Expansion object to populate - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void findBoundaryExpansion( - const geometry_msgs::msg::Pose & start, - const geometry_msgs::msg::Pose & end, - BoundaryExpansion & expansion, - const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Generates boundary expansions with end idx at least strategic - * distances away, using either Reverse or (Forward) Path Iterators. - * @param start iterator to start search in path for - * @param end iterator to end search for - * @return Boundary expansions with end idxs populated - */ - template - BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); - - /** - * @brief For a given path, update the path point orientations based on smoothing - * @param path Path to approximate the path orientation in - * @param reversing_segment Return if this is a reversing segment - */ - inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment); - - double min_turning_rad_, tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_, refinement_num_; - bool is_holonomic_, do_refinement_; - MotionModel motion_model_; - ompl::base::StateSpacePtr state_space_; -}; - -} // namespace nav2_smac_planner - -#endif // NAV2_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 9083dd61313..138cbf9765b 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -32,7 +32,6 @@ using std::placeholders::_1; SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), _collision_checker(nullptr, 1, nullptr), - _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) { @@ -132,20 +131,12 @@ void SmacPlanner2D::configure( 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); - // Initialize path smoother - SmootherParams params; - params.get(node, name); - params.holonomic_ = true; // So smoother will treat this as a grid search - _smoother = std::make_unique(params); - _smoother->initialize(1e-50 /*No valid minimum turning radius for 2D*/); - // Initialize costmap downsampler std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); _costmap_downsampler->on_configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); - _raw_plan_publisher = node->create_publisher("unsmoothed_plan"); RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlanner2D with " @@ -160,7 +151,6 @@ void SmacPlanner2D::activate() RCLCPP_INFO( _logger, "Activating plugin %s of type SmacPlanner2D", _name.c_str()); - _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { _costmap_downsampler->on_activate(); } @@ -175,7 +165,6 @@ void SmacPlanner2D::deactivate() RCLCPP_INFO( _logger, "Deactivating plugin %s of type SmacPlanner2D", _name.c_str()); - _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } @@ -193,12 +182,10 @@ void SmacPlanner2D::cleanup() _logger, "Cleaning up plugin %s of type SmacPlanner2D", _name.c_str()); _a_star.reset(); - _smoother.reset(); if (_costmap_downsampler) { _costmap_downsampler->on_cleanup(); _costmap_downsampler.reset(); } - _raw_plan_publisher.reset(); } nav_msgs::msg::Path SmacPlanner2D::createPlan( @@ -207,8 +194,6 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); - steady_clock::time_point a = steady_clock::now(); - std::unique_lock lock(*(_costmap->getMutex())); // Downsample costmap, if required @@ -271,11 +256,6 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( } plan.poses.push_back(pose); - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); - } - return plan; } @@ -306,24 +286,6 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( plan.poses.push_back(pose); } - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); - } - - // Find how much time we have left to do smoothing - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - double time_remaining = _max_planning_time - static_cast(time_span.count()); - -#ifdef BENCHMARK_TESTING - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - - // Smooth plan - _smoother->smooth(plan, costmap, time_remaining); - // If use_final_approach_orientation=true, interpolate the last pose orientation from the // previous pose to set the orientation to the 'final approach' orientation of the robot so // it does not rotate. diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 1f374af35d0..4c17ba9c710 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -33,7 +33,6 @@ using std::placeholders::_1; SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), _collision_checker(nullptr, 1, nullptr), - _smoother(nullptr), _costmap(nullptr), _costmap_ros(nullptr), _costmap_downsampler(nullptr) @@ -65,7 +64,6 @@ void SmacPlannerHybrid::configure( int angle_quantizations; double analytic_expansion_max_length_m; - bool smooth_path; // General planner params nav2::declare_parameter_if_not_declared( @@ -96,9 +94,6 @@ void SmacPlannerHybrid::configure( nav2::declare_parameter_if_not_declared( node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); - nav2::declare_parameter_if_not_declared( - node, name + ".smooth_path", rclcpp::ParameterValue(true)); - node->get_parameter(name + ".smooth_path", smooth_path); nav2::declare_parameter_if_not_declared( node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); @@ -272,29 +267,16 @@ void SmacPlannerHybrid::configure( _lookup_table_dim, _angle_quantizations); - // Initialize path smoother - if (smooth_path) { - SmootherParams params; - params.get(node, name); - _smoother = std::make_unique(params); - _smoother->initialize(_minimum_turning_radius_global_coords); - } - // Initialize costmap downsampler _costmap_downsampler = std::make_unique(); std::string topic_name = "downsampled_costmap"; _costmap_downsampler->on_configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); - _raw_plan_publisher = node->create_publisher("unsmoothed_plan"); - if (_debug_visualizations) { _expansions_publisher = node->create_publisher("expansions"); _planned_footprints_publisher = node->create_publisher( "planned_footprints"); - _smoothed_footprints_publisher = - node->create_publisher( - "smoothed_footprints"); } RCLCPP_INFO( @@ -311,11 +293,9 @@ void SmacPlannerHybrid::activate() RCLCPP_INFO( _logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); - _raw_plan_publisher->on_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(); @@ -340,11 +320,9 @@ void SmacPlannerHybrid::deactivate() RCLCPP_INFO( _logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); - _raw_plan_publisher->on_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(); @@ -364,16 +342,13 @@ void SmacPlannerHybrid::cleanup() _name.c_str()); nav2_smac_planner::NodeHybrid::destroyStaticAssets(); _a_star.reset(); - _smoother.reset(); if (_costmap_downsampler) { _costmap_downsampler->on_cleanup(); _costmap_downsampler.reset(); } - _raw_plan_publisher.reset(); if (_debug_visualizations) { _expansions_publisher.reset(); _planned_footprints_publisher.reset(); - _smoothed_footprints_publisher.reset(); } } @@ -383,8 +358,6 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); - steady_clock::time_point a = steady_clock::now(); - std::unique_lock lock(*(_costmap->getMutex())); // Downsample costmap, if required @@ -471,11 +444,6 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( pose.pose.orientation = goal.pose.orientation; plan.poses.push_back(pose); - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); - } - return plan; } @@ -526,11 +494,6 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( plan.poses.push_back(pose); } - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); - } - if (_debug_visualizations) { // Publish expansions for debug auto now = _clock->now(); @@ -554,7 +517,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( marker_array->markers.push_back(clear_all_marker); _planned_footprints_publisher->publish(std::move(marker_array)); - // Publish smoothed footprints for debug + // Publish planned footprints for debug marker_array = std::make_unique(); for (size_t i = 0; i < plan.poses.size(); i++) { const std::vector edge = @@ -565,49 +528,6 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } } - // Find how much time we have left to do smoothing - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - double time_remaining = _max_planning_time - static_cast(time_span.count()); - -#ifdef BENCHMARK_TESTING - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - - // Smooth plan - if (_smoother && num_iterations > 1) { - _smoother->smooth(plan, costmap, time_remaining); - } - -#ifdef BENCHMARK_TESTING - steady_clock::time_point c = steady_clock::now(); - duration time_span2 = duration_cast>(c - b); - std::cout << "It took " << time_span2.count() * 1000 << - " 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::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(); - auto now = _clock->now(); - for (size_t i = 0; i < plan.poses.size(); i++) { - const std::vector 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; } @@ -620,7 +540,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para bool reinit_collision_checker = false; bool reinit_a_star = false; bool reinit_downsampler = false; - bool reinit_smoother = false; for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); @@ -639,9 +558,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _lookup_table_size = parameter.as_double(); } else if (param_name == _name + ".minimum_turning_radius") { reinit_a_star = true; - if (_smoother) { - reinit_smoother = true; - } if (parameter.as_double() < _costmap->getResolution() * _downsampling_factor) { RCLCPP_ERROR( @@ -679,7 +595,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para reinit_collision_checker = true; reinit_a_star = true; reinit_downsampler = true; - reinit_smoother = true; } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == _name + ".downsample_costmap") { @@ -694,12 +609,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para } else if (param_name == _name + ".allow_primitive_interpolation") { _search_info.allow_primitive_interpolation = parameter.as_bool(); reinit_a_star = true; - } else if (param_name == _name + ".smooth_path") { - if (parameter.as_bool()) { - reinit_smoother = true; - } else { - _smoother.reset(); - } } else if (param_name == _name + ".analytic_expansion_max_cost_override") { _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); reinit_a_star = true; @@ -794,7 +703,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para } // Re-init if needed with mutex lock (to avoid re-init while creating a plan) - if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) { + if (reinit_a_star || reinit_downsampler || reinit_collision_checker) { // convert to grid coordinates if (!_downsample_costmap) { _downsampling_factor = 1; @@ -851,14 +760,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _costmap_ros->getUseRadius(), findCircumscribedCost(_costmap_ros)); } - - // Re-Initialize smoother - if (reinit_smoother) { - SmootherParams params; - params.get(node, _name); - _smoother = std::make_unique(params); - _smoother->initialize(_minimum_turning_radius_global_coords); - } } result.successful = true; return result; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index fa89e90636c..94de5b61c42 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -31,7 +31,6 @@ using rcl_interfaces::msg::ParameterType; SmacPlannerLattice::SmacPlannerLattice() : _a_star(nullptr), _collision_checker(nullptr, 1, nullptr), - _smoother(nullptr), _costmap(nullptr) { } @@ -61,7 +60,6 @@ void SmacPlannerLattice::configure( // General planner params double analytic_expansion_max_length_m; - bool smooth_path; nav2::declare_parameter_if_not_declared( node, name + ".tolerance", rclcpp::ParameterValue(0.25)); @@ -78,9 +76,6 @@ void SmacPlannerLattice::configure( nav2::declare_parameter_if_not_declared( node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); - nav2::declare_parameter_if_not_declared( - node, name + ".smooth_path", rclcpp::ParameterValue(true)); - node->get_parameter(name + ".smooth_path", smooth_path); // Default to a well rounded model: 16 bin, 0.4m turning radius, ackermann model nav2::declare_parameter_if_not_declared( @@ -229,23 +224,10 @@ void SmacPlannerLattice::configure( lookup_table_dim, _metadata.number_of_headings); - // Initialize path smoother - if (smooth_path) { - SmootherParams params; - params.get(node, name); - _smoother = std::make_unique(params); - _smoother->initialize(_metadata.min_turning_radius); - } - - _raw_plan_publisher = node->create_publisher("unsmoothed_plan"); - if (_debug_visualizations) { _expansions_publisher = node->create_publisher("expansions"); _planned_footprints_publisher = node->create_publisher( "planned_footprints"); - _smoothed_footprints_publisher = - node->create_publisher( - "smoothed_footprints"); } RCLCPP_INFO( @@ -262,11 +244,9 @@ void SmacPlannerLattice::activate() RCLCPP_INFO( _logger, "Activating plugin %s of type SmacPlannerLattice", _name.c_str()); - _raw_plan_publisher->on_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 @@ -279,11 +259,9 @@ void SmacPlannerLattice::deactivate() RCLCPP_INFO( _logger, "Deactivating plugin %s of type SmacPlannerLattice", _name.c_str()); - _raw_plan_publisher->on_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(); @@ -300,12 +278,9 @@ void SmacPlannerLattice::cleanup() _name.c_str()); nav2_smac_planner::NodeHybrid::destroyStaticAssets(); _a_star.reset(); - _smoother.reset(); - _raw_plan_publisher.reset(); if (_debug_visualizations) { _expansions_publisher.reset(); _planned_footprints_publisher.reset(); - _smoothed_footprints_publisher.reset(); } } @@ -315,8 +290,6 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); - steady_clock::time_point a = steady_clock::now(); - std::unique_lock lock(*(_costmap->getMutex())); // Set collision checker and costmap information @@ -380,11 +353,6 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( pose.pose.orientation = goal.pose.orientation; plan.poses.push_back(pose); - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); - } - return plan; } @@ -449,11 +417,6 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( plan.poses.push_back(pose); } - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); - } - if (_debug_visualizations) { auto now = _clock->now(); // Publish expansions for debug @@ -477,7 +440,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( marker_array->markers.push_back(clear_all_marker); _planned_footprints_publisher->publish(std::move(marker_array)); - // Publish smoothed footprints for debug + // Publish planned footprints for debug marker_array = std::make_unique(); for (size_t i = 0; i < plan.poses.size(); i++) { const std::vector edge = @@ -488,49 +451,6 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } } - // Find how much time we have left to do smoothing - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - double time_remaining = _max_planning_time - static_cast(time_span.count()); - -#ifdef BENCHMARK_TESTING - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - - // Smooth plan - if (_smoother && num_iterations > 1) { - _smoother->smooth(plan, _costmap, time_remaining); - } - -#ifdef BENCHMARK_TESTING - steady_clock::time_point c = steady_clock::now(); - duration time_span2 = duration_cast>(c - b); - std::cout << "It took " << time_span2.count() * 1000 << - " 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::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(); - auto now = _clock->now(); - for (size_t i = 0; i < plan.poses.size(); i++) { - const std::vector 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; } @@ -541,7 +461,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par std::lock_guard lock_reinit(_mutex); bool reinit_a_star = false; - bool reinit_smoother = false; for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); @@ -594,12 +513,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else if (param_name == _name + ".allow_reverse_expansion") { reinit_a_star = true; _search_info.allow_reverse_expansion = parameter.as_bool(); - } else if (param_name == _name + ".smooth_path") { - if (parameter.as_bool()) { - reinit_smoother = true; - } else { - _smoother.reset(); - } } else if (param_name == _name + ".analytic_expansion_max_cost_override") { _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); reinit_a_star = true; @@ -647,9 +560,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".lattice_filepath") { reinit_a_star = true; - if (_smoother) { - reinit_smoother = true; - } _search_info.lattice_filepath = parameter.as_string(); _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = @@ -682,7 +592,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } // Re-init if needed with mutex lock (to avoid re-init while creating a plan) - if (reinit_a_star || reinit_smoother) { + if (reinit_a_star) { // convert to grid coordinates _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -702,15 +612,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par lookup_table_dim += 1.0; } - // Re-Initialize smoother - if (reinit_smoother) { - auto node = _node.lock(); - SmootherParams params; - params.get(node, _name); - _smoother = std::make_unique(params); - _smoother->initialize(_metadata.min_turning_radius); - } - // Re-Initialize A* template if (reinit_a_star) { _a_star = std::make_unique>(_motion_model, _search_info); diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp deleted file mode 100644 index be417fa1be7..00000000000 --- a/nav2_smac_planner/src/smoother.cpp +++ /dev/null @@ -1,520 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include - -#include -#include -#include - -#include "angles/angles.h" - -#include "tf2/utils.hpp" - -#include "nav2_smac_planner/smoother.hpp" - -namespace nav2_smac_planner -{ -using namespace nav2_util::geometry_utils; // NOLINT -using namespace std::chrono; // NOLINT - -Smoother::Smoother(const SmootherParams & params) -{ - tolerance_ = params.tolerance_; - max_its_ = params.max_its_; - data_w_ = params.w_data_; - smooth_w_ = params.w_smooth_; - is_holonomic_ = params.holonomic_; - do_refinement_ = params.do_refinement_; - refinement_num_ = params.refinement_num_; -} - -void Smoother::initialize(const double & min_turning_radius) -{ - min_turning_rad_ = min_turning_radius; - state_space_ = std::make_unique(min_turning_rad_); -} - -bool Smoother::smooth( - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time) -{ - // by-pass path orientations approximation when skipping smac smoother - if (max_its_ == 0) { - return false; - } - - steady_clock::time_point start = steady_clock::now(); - double time_remaining = max_time; - bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; - curr_path_segment.header = path.header; - std::vector path_segments = findDirectionalPathSegments(path); - - for (unsigned int i = 0; i != path_segments.size(); i++) { - if (path_segments[i].end - path_segments[i].start > 10) { - // Populate path segment - curr_path_segment.poses.clear(); - std::copy( - path.poses.begin() + path_segments[i].start, - path.poses.begin() + path_segments[i].end + 1, - std::back_inserter(curr_path_segment.poses)); - - // Make sure we're still able to smooth with time remaining - steady_clock::time_point now = steady_clock::now(); - time_remaining = max_time - duration_cast>(now - start).count(); - refinement_ctr_ = 0; - - // Smooth path segment naively - 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); - success = success && local_success; - - // Enforce boundary conditions - if (!is_holonomic_ && local_success) { - enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment); - enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment); - } - - // Assemble the path changes to the main path - std::copy( - curr_path_segment.poses.begin(), - curr_path_segment.poses.end(), - path.poses.begin() + path_segments[i].start); - } - } - - return success; -} - -bool Smoother::smoothImpl( - nav_msgs::msg::Path & path, - bool & reversing_segment, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time) -{ - steady_clock::time_point a = steady_clock::now(); - rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); - - int its = 0; - double change = tolerance_; - const unsigned int & path_size = path.poses.size(); - double x_i, y_i, y_m1, y_ip1, y_i_org; - unsigned int mx, my; - - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; - - while (change >= tolerance_) { - its += 1; - change = 0.0; - - // Make sure the smoothing function will converge - if (its >= max_its_) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Number of iterations has exceeded limit of %i.", max_its_); - path = last_path; - updateApproximatePathOrientations(path, reversing_segment); - return false; - } - - // Make sure still have time left to process - steady_clock::time_point b = steady_clock::now(); - rclcpp::Duration timespan(duration_cast>(b - a)); - if (timespan > max_dur) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing time exceeded allowed duration of %0.2f.", max_time); - path = last_path; - updateApproximatePathOrientations(path, reversing_segment); - return false; - } - - for (unsigned int i = 1; i != path_size - 1; i++) { - for (unsigned int j = 0; j != 2; j++) { - x_i = getFieldByDim(path.poses[i], j); - y_i = getFieldByDim(new_path.poses[i], j); - y_m1 = getFieldByDim(new_path.poses[i - 1], j); - y_ip1 = getFieldByDim(new_path.poses[i + 1], j); - y_i_org = y_i; - - // Smooth based on local 3 point neighborhood and original data locations - y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); - setFieldByDim(new_path.poses[i], j, y_i); - change += abs(y_i - y_i_org); - } - - // validate update is admissible, only checks cost if a valid costmap pointer is provided - float cost = 0.0; - if (costmap) { - costmap->worldToMap( - getFieldByDim(new_path.poses[i], 0), - getFieldByDim(new_path.poses[i], 1), - mx, my); - cost = static_cast(costmap->getCost(mx, my)); - } - - if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing process resulted in an infeasible collision. " - "Returning the last path before the infeasibility was introduced."); - path = last_path; - updateApproximatePathOrientations(path, reversing_segment); - return false; - } - } - - last_path = new_path; - } - - // Let's do additional refinement, it shouldn't take more than a couple milliseconds - // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < refinement_num_) { - refinement_ctr_++; - smoothImpl(new_path, reversing_segment, costmap, max_time); - } - - updateApproximatePathOrientations(new_path, reversing_segment); - path = new_path; - return true; -} - -double Smoother::getFieldByDim( - const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) -{ - if (dim == 0) { - return msg.pose.position.x; - } else if (dim == 1) { - return msg.pose.position.y; - } else { - return msg.pose.position.z; - } -} - -void Smoother::setFieldByDim( - geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, - const double & value) -{ - if (dim == 0) { - msg.pose.position.x = value; - } else if (dim == 1) { - msg.pose.position.y = value; - } else { - msg.pose.position.z = value; - } -} - -std::vector Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path) -{ - std::vector segments; - PathSegment curr_segment; - curr_segment.start = 0; - - // If holonomic, no directional changes and - // may have abrupt angular changes from naive grid search - if (is_holonomic_) { - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; - } - - // Iterating through the path to determine the position of the cusp - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - - // Checking for the existence of a differential rotation in place. - double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); - double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); - double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); - if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - } - - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; -} - -void Smoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment) -{ - double dx, dy, theta, pt_yaw; - reversing_segment = false; - - // Find if this path segment is in reverse - dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; - dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; - theta = atan2(dy, dx); - pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { - reversing_segment = true; - } - - // Find the angle relative the path position vectors - for (unsigned int i = 0; i != path.poses.size() - 1; i++) { - dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; - dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; - theta = atan2(dy, dx); - - // If points are overlapping, pass - if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { - continue; - } - - // Flip the angle if this path segment is in reverse - if (reversing_segment) { - theta += M_PI; // orientationAroundZAxis will normalize - } - - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } -} - -unsigned int Smoother::findShortestBoundaryExpansionIdx( - const BoundaryExpansions & boundary_expansions) -{ - // Check which is valid with the minimum integrated length such that - // shorter end-points away that are infeasible to achieve without - // a loop-de-loop are punished - double min_length = 1e9; - int shortest_boundary_expansion_idx = 1e9; - for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { - if (boundary_expansions[idx].expansion_path_length0.0 && - boundary_expansions[idx].expansion_path_length > 0.0) - { - min_length = boundary_expansions[idx].expansion_path_length; - shortest_boundary_expansion_idx = idx; - } - } - - return shortest_boundary_expansion_idx; -} - -void Smoother::findBoundaryExpansion( - const geometry_msgs::msg::Pose & start, - const geometry_msgs::msg::Pose & end, - BoundaryExpansion & expansion, - const nav2_costmap_2d::Costmap2D * costmap) -{ - static ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_); - - from[0] = start.position.x; - from[1] = start.position.y; - from[2] = tf2::getYaw(start.orientation); - to[0] = end.position.x; - to[1] = end.position.y; - to[2] = tf2::getYaw(end.orientation); - - double d = state_space_->distance(from(), to()); - // If this path is too long compared to the original, then this is probably - // a loop-de-loop, treat as invalid as to not deviate too far from the original path. - // 2.0 selected from prinicipled choice of boundary test points - // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be - // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. - // For all but the last backup test point, a loop would be approximately - // 2x greater than any of the selections. - if (d > 2.0 * expansion.original_path_length) { - return; - } - - std::vector reals; - double theta(0.0), x(0.0), y(0.0); - double x_m = start.position.x; - double y_m = start.position.y; - - // Get intermediary poses - for (double i = 0; i <= expansion.path_end_idx; i++) { - state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s()); - reals = s.reals(); - // Make sure in range [0, 2PI) - theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; - theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; - x = reals[0]; - y = reals[1]; - - // Check for collision - unsigned int mx, my; - costmap->worldToMap(x, y, mx, my); - if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED_COST) { - expansion.in_collision = true; - } - - // Integrate path length - expansion.expansion_path_length += hypot(x - x_m, y - y_m); - x_m = x; - y_m = y; - - // Store point - expansion.pts.emplace_back(x, y, theta); - } -} - -template -BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end) -{ - std::vector distances = { - min_turning_rad_, // Radius - 2.0 * min_turning_rad_, // Diameter - M_PI * min_turning_rad_, // 50% Circumference - 2.0 * M_PI * min_turning_rad_ // Circumference - }; - - BoundaryExpansions boundary_expansions; - boundary_expansions.resize(distances.size()); - double curr_dist = 0.0; - double x_last = start->pose.position.x; - double y_last = start->pose.position.y; - geometry_msgs::msg::Point pt; - unsigned int curr_dist_idx = 0; - - for (IteratorT iter = start; iter != end; iter++) { - pt = iter->pose.position; - curr_dist += hypot(pt.x - x_last, pt.y - y_last); - x_last = pt.x; - y_last = pt.y; - - if (curr_dist >= distances[curr_dist_idx]) { - boundary_expansions[curr_dist_idx].path_end_idx = iter - start; - boundary_expansions[curr_dist_idx].original_path_length = curr_dist; - curr_dist_idx++; - } - - if (curr_dist_idx == boundary_expansions.size()) { - break; - } - } - - return boundary_expansions; -} - -void Smoother::enforceStartBoundaryConditions( - const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment) -{ - // Find range of points for testing - BoundaryExpansions boundary_expansions = - generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end()); - - // Generate the motion model and metadata from start -> test points - for (unsigned int i = 0; i != boundary_expansions.size(); i++) { - BoundaryExpansion & expansion = boundary_expansions[i]; - if (expansion.path_end_idx == 0.0) { - continue; - } - - if (!reversing_segment) { - findBoundaryExpansion( - start_pose, path.poses[expansion.path_end_idx].pose, expansion, - costmap); - } else { - findBoundaryExpansion( - path.poses[expansion.path_end_idx].pose, start_pose, expansion, - costmap); - } - } - - // Find the shortest kinematically feasible boundary expansion - unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); - if (best_expansion_idx > boundary_expansions.size()) { - return; - } - - // Override values to match curve - BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; - if (reversing_segment) { - std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); - } - for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { - path.poses[i].pose.position.x = best_expansion.pts[i].x; - path.poses[i].pose.position.y = best_expansion.pts[i].y; - path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); - } -} - -void Smoother::enforceEndBoundaryConditions( - const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment) -{ - // Find range of points for testing - BoundaryExpansions boundary_expansions = - generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend()); - - // Generate the motion model and metadata from start -> test points - unsigned int expansion_starting_idx; - for (unsigned int i = 0; i != boundary_expansions.size(); i++) { - BoundaryExpansion & expansion = boundary_expansions[i]; - if (expansion.path_end_idx == 0.0) { - continue; - } - expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; - if (!reversing_segment) { - findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap); - } else { - findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap); - } - } - - // Find the shortest kinematically feasible boundary expansion - unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); - if (best_expansion_idx > boundary_expansions.size()) { - return; - } - - // Override values to match curve - BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; - if (reversing_segment) { - std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); - } - expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; - for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { - path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; - path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; - path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( - best_expansion.pts[i].theta); - } -} - -} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 24c86d9c24f..836aa30d128 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -111,21 +111,6 @@ target_link_libraries(test_smac_lattice rclcpp_lifecycle::rclcpp_lifecycle ) -# Test SMAC Smoother -ament_add_gtest(test_smoother - test_smoother.cpp -) -target_link_libraries(test_smoother - ${library_name}_lattice - ${library_name} - ${library_name}_2d - ament_index_cpp::ament_index_cpp - ${geometry_msgs_TARGETS} - nav2_costmap_2d::nav2_costmap_2d_core - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle -) - # Test Lattice node ament_add_gtest(test_lattice_node test_nodelattice.cpp) target_link_libraries(test_lattice_node diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index bbad2044d3f..7f47829b1b0 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -42,8 +42,6 @@ TEST(SmacTest, test_smac_2d) { std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - node2D->declare_parameter("test.smooth_path", true); - node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true)); node2D->declare_parameter("test.downsample_costmap", true); node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); node2D->declare_parameter("test.downsampling_factor", 2); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index f356bf04227..6b68e8a17c3 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -198,7 +198,6 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), rclcpp::Parameter("test.max_planning_time", 10.0), rclcpp::Parameter("test.lookup_table_size", 30.0), - rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.terminal_checking_interval", 42), @@ -224,7 +223,6 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2); EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0); - EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0); EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index d9f33f7050e..b5deffc7e2f 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -174,7 +174,6 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), rclcpp::Parameter("test.max_planning_time", 10.0), rclcpp::Parameter("test.lookup_table_size", 30.0), - rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp deleted file mode 100644 index 295ce7e93a8..00000000000 --- a/nav2_smac_planner/test/test_smoother.cpp +++ /dev/null @@ -1,227 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" -#include "nav2_smac_planner/a_star.hpp" -#include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smoother.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" - -using namespace nav2_smac_planner; // NOLINT - -class SmootherWrapper : public nav2_smac_planner::Smoother -{ -public: - explicit SmootherWrapper(const SmootherParams & params) - : nav2_smac_planner::Smoother(params) - {} - - std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) - { - return findDirectionalPathSegments(path); - } -}; - -TEST(SmootherTest, test_full_smoother) -{ - nav2::LifecycleNode::SharedPtr node = - std::make_shared("SmacSmootherTest"); - nav2_smac_planner::SmootherParams params; - params.get(node, "test"); - double maxtime = 1.0; - - // Make smoother and costmap to smooth in - auto smoother = std::make_unique(params); - smoother->initialize(0.4 /*turning radius*/); - - nav2_costmap_2d::Costmap2D * costmap = - new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0); - // island in the middle of lethal cost to cross - for (unsigned int i = 20; i <= 30; ++i) { - for (unsigned int j = 20; j <= 30; ++j) { - costmap->setCost(i, j, 254); - } - } - - // Setup A* search to get path to smooth - nav2_smac_planner::SearchInfo info; - info.change_penalty = 0.05; - info.non_straight_penalty = 1.05; - info.reverse_penalty = 2.0; - info.cost_penalty = 2.0; - info.retrospective_penalty = 0.0; - info.analytic_expansion_ratio = 3.5; - info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05 - info.analytic_expansion_max_length = 20.0; // in grid coordinates - unsigned int size_theta = 72; - nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::REEDS_SHEPP, info); - int max_iterations = 10000; - float tolerance = 0.0; - int terminal_checking_interval = 5000; - double max_planning_time = 120.0; - int num_it = 0; - - a_star.initialize( - false, max_iterations, - std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, - size_theta); - - // Convert raw costmap into a costmap ros object - auto costmap_ros = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto costmapi = costmap_ros->getCostmap(); - *costmapi = *costmap; - - std::unique_ptr checker = - std::make_unique(costmap_ros, size_theta, node); - checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - - auto dummy_cancel_checker = []() { - return false; - }; - - // Create A* search to smooth - a_star.setCollisionChecker(checker.get()); - a_star.setStart(5u, 5u, 0u); - a_star.setGoal(45u, 45u, 36u); - nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); - - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), "###############"); - - // Convert to world coordinates and get length to compare to smoothed length - nav_msgs::msg::Path plan; - plan.header.stamp = node->now(); - plan.header.frame_id = "map"; - geometry_msgs::msg::PoseStamped pose; - pose.header = plan.header; - pose.pose.position.z = 0.0; - pose.pose.orientation.x = 0.0; - pose.pose.orientation.y = 0.0; - pose.pose.orientation.z = 0.0; - pose.pose.orientation.w = 1.0; - double initial_length = 0.0; - double x_m = path[path.size() - 1].x, y_m = path[path.size() - 1].y; - plan.poses.reserve(path.size()); - for (int i = path.size() - 1; i >= 0; --i) { - pose.pose = nav2_smac_planner::getWorldCoords(path[i].x, path[i].y, costmap); - pose.pose.orientation = nav2_smac_planner::getWorldOrientation(path[i].theta); - plan.poses.push_back(pose); - initial_length += hypot(path[i].x - x_m, path[i].y - y_m); - x_m = path[i].x; - y_m = path[i].y; - - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), - "smac_path.poses[%ld].pose.position.x = %f;", path.size() - i - 1, pose.pose.position.x); - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), - "smac_path.poses[%ld].pose.position.y = %f;", path.size() - i - 1, pose.pose.position.y); - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), - "smac_path.poses[%ld].pose.orientation.x = %f;", path.size() - i - 1, pose.pose.orientation.x); - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), - "smac_path.poses[%ld].pose.orientation.y = %f;", path.size() - i - 1, pose.pose.orientation.y); - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), - "smac_path.poses[%ld].pose.orientation.z = %f;", path.size() - i - 1, pose.pose.orientation.z); - RCLCPP_WARN( - rclcpp::get_logger("SmacPlannerSmoother"), - "smac_path.poses[%ld].pose.orientation.w = %f;", path.size() - i - 1, pose.pose.orientation.w); - } - - // Check that we accurately detect that this path has a reversing segment - auto path_segs = smoother->findDirectionalPathSegmentsWrapper(plan); - EXPECT_TRUE(path_segs.size() == 2u || path_segs.size() == 3u); - - // Test smoother, should succeed with same number of points - // and shorter overall length, while still being collision free. - auto path_size_in = plan.poses.size(); - EXPECT_TRUE(smoother->smooth(plan, costmap, maxtime)); - EXPECT_EQ(plan.poses.size(), path_size_in); // Should have same number of poses - double length = 0.0; - x_m = plan.poses[0].pose.position.x; - y_m = plan.poses[0].pose.position.y; - for (unsigned int i = 0; i != plan.poses.size(); i++) { - // Should be collision free - EXPECT_EQ(costmap->getCost(plan.poses[i].pose.position.x, plan.poses[i].pose.position.y), 0); - length += hypot(plan.poses[i].pose.position.x - x_m, plan.poses[i].pose.position.y - y_m); - x_m = plan.poses[i].pose.position.x; - y_m = plan.poses[i].pose.position.y; - } - EXPECT_LT(length, initial_length); // Should be shorter - - // Try again but with failure modes - - // Failure mode: not enough iterations to complete - params.max_its_ = 0; - auto smoother_bypass = std::make_unique(params); - EXPECT_FALSE(smoother_bypass->smooth(plan, costmap, maxtime)); - params.max_its_ = 1; - auto smoother_failure = std::make_unique(params); - EXPECT_FALSE(smoother_failure->smooth(plan, costmap, maxtime)); - - // Failure mode: Not enough time - double max_no_time = 0.0; - EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time)); - - // Failure mode: invalid path and invalid orientation - // make the end of the path invalid - geometry_msgs::msg::PoseStamped lastPose = plan.poses.back(); - unsigned int mx, my; - costmap->worldToMap(lastPose.pose.position.x, lastPose.pose.position.y, mx, my); - for (unsigned int i = mx - 5; i <= mx + 5; ++i) { - for (unsigned int j = my - 5; j <= my + 5; ++j) { - costmap->setCost(i, j, 254); - } - } - - // duplicate last pose to make the orientation update fail - plan.poses.push_back(lastPose); - EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime)); - EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3); - EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3); - EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.y, 0.0, 1e-3); - EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3); - - delete costmap; - nav2_smac_planner::NodeHybrid::destroyStaticAssets(); -} - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - rclcpp::init(0, nullptr); - - int result = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - - return result; -} diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 518edd5a8ed..150d1c0c5bc 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -63,16 +63,16 @@ void SimpleSmoother::configure( node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); - - if (!is_holonomic_) { - state_space_ = std::make_unique(min_turning_radius_); - } } bool SimpleSmoother::smooth( nav_msgs::msg::Path & path, const rclcpp::Duration & max_time) { + if (!is_holonomic_) { + state_space_ = std::make_unique(min_turning_radius_); + } + auto costmap = costmap_sub_->getCostmap(); steady_clock::time_point start = steady_clock::now(); diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index 376c28da733..4b4038d7e70 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -41,12 +41,23 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother return findDirectionalPathSegments(path); } - void setMaxItsToInvalid() + void setMaxIts(const int max_its) { - max_its_ = 0; + max_its_ = max_its; } - double getPathLength(const nav_msgs::msg::Path & path) { + void setIsHolonomic(const bool is_holonomic) + { + is_holonomic_ = is_holonomic; + } + + void setMinimumTurningRadius(const double min_turning_radius) + { + min_turning_radius_ = min_turning_radius; + } + + double getPathLength(const nav_msgs::msg::Path & path) + { if (path.poses.size() == 0) { return 0.0; } @@ -262,7 +273,40 @@ TEST(SmootherTest, test_simple_smoother) approach_path.poses[2].pose.position.y = 0.2; EXPECT_TRUE(smoother->smooth(approach_path, max_time)); + // test max iterations + smoother->setMaxIts(0.0); + nav_msgs::msg::Path max_its_path; + max_its_path.poses.resize(11); + max_its_path.poses[0].pose.position.x = 0.5; + max_its_path.poses[0].pose.position.y = 0.0; + max_its_path.poses[1].pose.position.x = 0.5; + max_its_path.poses[1].pose.position.y = 0.1; + max_its_path.poses[2].pose.position.x = 0.5; + max_its_path.poses[2].pose.position.y = 0.2; + max_its_path.poses[3].pose.position.x = 0.5; + max_its_path.poses[3].pose.position.y = 0.3; + max_its_path.poses[4].pose.position.x = 0.5; + max_its_path.poses[4].pose.position.y = 0.4; + max_its_path.poses[5].pose.position.x = 0.5; + max_its_path.poses[5].pose.position.y = 0.5; + max_its_path.poses[6].pose.position.x = 0.5; + max_its_path.poses[6].pose.position.y = 0.6; + max_its_path.poses[7].pose.position.x = 0.5; + max_its_path.poses[7].pose.position.y = 0.7; + max_its_path.poses[8].pose.position.x = 0.5; + max_its_path.poses[8].pose.position.y = 0.8; + max_its_path.poses[9].pose.position.x = 0.5; + 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_FALSE(smoother->smooth(max_its_path, max_time)); + + // test nonholonomic + smoother->setIsHolonomic(false); + smoother->setMinimumTurningRadius(0.4); + smoother->setMaxIts(1000); nav_msgs::msg::Path smac_path; + smac_path.poses.resize(37); smac_path.poses[0].pose.position.x = 0.250000; smac_path.poses[0].pose.position.y = 0.250000; smac_path.poses[0].pose.orientation.x = 0.000000; @@ -497,34 +541,6 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_TRUE(smoother->smooth(smac_path, max_time)); EXPECT_EQ(smac_path.poses.size(), path_size_in); // Should have same number of poses EXPECT_LT(smoother->getPathLength(smac_path), initial_length); // Should be shorter - - // test max iterations - smoother->setMaxItsToInvalid(); - nav_msgs::msg::Path max_its_path; - max_its_path.poses.resize(11); - max_its_path.poses[0].pose.position.x = 0.5; - max_its_path.poses[0].pose.position.y = 0.0; - max_its_path.poses[1].pose.position.x = 0.5; - max_its_path.poses[1].pose.position.y = 0.1; - max_its_path.poses[2].pose.position.x = 0.5; - max_its_path.poses[2].pose.position.y = 0.2; - max_its_path.poses[3].pose.position.x = 0.5; - max_its_path.poses[3].pose.position.y = 0.3; - max_its_path.poses[4].pose.position.x = 0.5; - max_its_path.poses[4].pose.position.y = 0.4; - max_its_path.poses[5].pose.position.x = 0.5; - max_its_path.poses[5].pose.position.y = 0.5; - max_its_path.poses[6].pose.position.x = 0.5; - max_its_path.poses[6].pose.position.y = 0.6; - max_its_path.poses[7].pose.position.x = 0.5; - max_its_path.poses[7].pose.position.y = 0.7; - max_its_path.poses[8].pose.position.x = 0.5; - max_its_path.poses[8].pose.position.y = 0.8; - max_its_path.poses[9].pose.position.x = 0.5; - 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_FALSE(smoother->smooth(max_its_path, max_time)); } int main(int argc, char **argv)