From 2b1c02cdd8f09d14019c1f21233f0369ecc8b1a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20=C3=96rjehag?= Date: Mon, 26 Feb 2024 10:10:14 +0100 Subject: [PATCH 1/2] Implemented changes from ros-2-main into controller and pure pursuit --- nav2_controller/CMakeLists.txt | 4 + .../nav2_controller/nav2_controller.hpp | 13 + .../plugins/simple_goal_checker.hpp | 3 + nav2_controller/package.xml | 1 + .../plugins/simple_goal_checker.cpp | 36 ++- nav2_controller/src/nav2_controller.cpp | 133 ++++++--- .../CMakeLists.txt | 14 +- .../regulated_pure_pursuit_controller.hpp | 36 ++- .../src/regulated_pure_pursuit_controller.cpp | 275 +++++++++++++++--- nav2_smac_planner/CMakeLists.txt | 12 +- .../include/nav2_smac_planner/a_star.hpp | 8 +- .../include/nav2_smac_planner/utils.hpp | 11 + nav2_smac_planner/src/a_star.cpp | 30 +- nav2_smac_planner/src/smac_planner_2d.cpp | 4 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 10 +- 15 files changed, 477 insertions(+), 113 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 8a7b1da217e..d4ed7ea055e 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -13,6 +13,8 @@ find_package(nav2_msgs REQUIRED) find_package(nav_2d_utils REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(relox_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) nav2_package() @@ -45,6 +47,8 @@ set(dependencies nav2_util nav2_core pluginlib + relox_msgs + diagnostic_updater ) add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 77e20d93e45..084f954cae0 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -15,6 +15,7 @@ #ifndef NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_ #define NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_ +#include #include #include #include @@ -34,6 +35,7 @@ #include "nav2_util/robot_utils.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" +#include "relox_msgs/msg/safety_status.hpp" namespace nav2_controller { @@ -208,6 +210,7 @@ class ControllerServer : public nav2_util::LifecycleNode std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; rclcpp::Subscription::SharedPtr speed_limit_sub_; + rclcpp::Subscription::SharedPtr safety_status_sub_; // Progress Checker Plugin pluginlib::ClassLoader progress_checker_loader_; @@ -235,7 +238,13 @@ class ControllerServer : public nav2_util::LifecycleNode std::vector controller_types_; std::string controller_ids_concat_, current_controller_; + void init_diagnostic_updater_(); + void check_rate(diagnostic_updater::DiagnosticStatusWrapper & status); + std::unique_ptr diagnostics_updater_; + double controller_frequency_; + int controller_frequency_trigger_; + int CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD = 20; double min_x_velocity_threshold_; double min_y_velocity_threshold_; double min_theta_velocity_threshold_; @@ -251,12 +260,16 @@ class ControllerServer : public nav2_util::LifecycleNode // Current path container nav_msgs::msg::Path current_path_; + bool is_safety_status_ok_{false}; + private: /** * @brief Callback for speed limiting messages * @param msg Shared pointer to nav2_msgs::msg::SpeedLimit */ void speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg); + + void safetyStatusCallback(const relox_msgs::msg::SafetyStatus::SharedPtr msg); }; } // namespace nav2_controller diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 705ee2a0af7..357af3046e3 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -78,6 +78,9 @@ class SimpleGoalChecker : public nav2_core::GoalChecker rclcpp::Subscription::SharedPtr parameter_event_sub_; std::string plugin_name_; + // Make sure we are outside the tolerance of the goal before we enter inside the tolerance + bool has_left_goal_; + /** * @brief Callback executed when a paramter change is detected * @param event ParameterEvent message diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index bc4a51cc32e..63ba8d71c84 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -19,6 +19,7 @@ nav_2d_msgs nav2_core pluginlib + relox_msgs ament_lint_common ament_lint_auto diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index e03ce8b421c..367ec32375d 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -55,8 +55,9 @@ SimpleGoalChecker::SimpleGoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), - check_xy_(true), - xy_goal_tolerance_sq_(0.0625) + check_xy_(true), + xy_goal_tolerance_sq_(0.0625), + has_left_goal_{false} { } @@ -97,28 +98,37 @@ void SimpleGoalChecker::initialize( void SimpleGoalChecker::reset() { check_xy_ = true; + has_left_goal_ = false; } bool SimpleGoalChecker::isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist &) { - if (check_xy_) { + if (has_left_goal_ == false) { double dx = query_pose.position.x - goal_pose.position.x, - dy = query_pose.position.y - goal_pose.position.y; + dy = query_pose.position.y - goal_pose.position.y; if (dx * dx + dy * dy > xy_goal_tolerance_sq_) { - return false; + has_left_goal_ = true; } - // We are within the window - // If we are stateful, change the state. - if (stateful_) { - check_xy_ = false; + return false; + } else { + if (check_xy_) { + double dx = query_pose.position.x - goal_pose.position.x, + dy = query_pose.position.y - goal_pose.position.y; + if (dx * dx + dy * dy > xy_goal_tolerance_sq_) { + return false; + } + // We are within the window + // If we are stateful, change the state. + if (stateful_) { + check_xy_ = false; + } } + double dyaw = angles::shortest_angular_distance( + tf2::getYaw(query_pose.orientation), tf2::getYaw(goal_pose.orientation)); + return fabs(dyaw) < yaw_goal_tolerance_; } - double dyaw = angles::shortest_angular_distance( - tf2::getYaw(query_pose.orientation), - tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) < yaw_goal_tolerance_; } bool SimpleGoalChecker::getTolerances( diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index e23ea1f7e94..8a724577b11 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -41,7 +41,9 @@ ControllerServer::ControllerServer() default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, - default_types_{"dwb_core::DWBLocalPlanner"} + default_types_{"dwb_core::DWBLocalPlanner"}, + diagnostics_updater_(new diagnostic_updater::Updater(this, 0.5)), + controller_frequency_trigger_(0) { RCLCPP_INFO(get_logger(), "Creating controller server"); @@ -74,11 +76,30 @@ ControllerServer::~ControllerServer() costmap_thread_.reset(); } +void ControllerServer::init_diagnostic_updater_() +{ + diagnostics_updater_->setHardwareID("nav2_controller"); + diagnostics_updater_->add("Nav2 Rate Low", this, &ControllerServer::check_rate); + diagnostics_updater_->broadcast( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data in nav2 monitor yet"); +} + +void ControllerServer::check_rate(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + if (controller_frequency_trigger_ >= CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD) { + status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 multiple rate misses"); + } else { + status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 rate OK"); + } +} + nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); + init_diagnostic_updater_(); + RCLCPP_INFO(get_logger(), "Configuring controller interface"); get_parameter("progress_checker_plugin", progress_checker_id_); @@ -203,6 +224,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) speed_limit_topic, rclcpp::QoS(10), std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); + safety_status_sub_ = create_subscription( + "/safety_status", rclcpp::QoS(10), + std::bind(&ControllerServer::safetyStatusCallback, this, std::placeholders::_1)); + return nav2_util::CallbackReturn::SUCCESS; } @@ -266,6 +291,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) odom_sub_.reset(); vel_publisher_.reset(); speed_limit_sub_.reset(); + safety_status_sub_.reset(); action_server_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -341,6 +367,7 @@ void ControllerServer::computeControl() current_controller_ = current_controller; } else { action_server_->terminate_current(); + controller_frequency_trigger_ = 0; return; } @@ -350,6 +377,7 @@ void ControllerServer::computeControl() current_goal_checker_ = current_goal_checker; } else { action_server_->terminate_current(); + controller_frequency_trigger_ = 0; return; } @@ -361,6 +389,7 @@ void ControllerServer::computeControl() while (rclcpp::ok()) { if (action_server_ == nullptr || !action_server_->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + controller_frequency_trigger_ = 0; return; } @@ -368,6 +397,7 @@ void ControllerServer::computeControl() RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot."); action_server_->terminate_all(); publishZeroVelocity(); + controller_frequency_trigger_ = 0; return; } @@ -387,10 +417,24 @@ void ControllerServer::computeControl() } if (!loop_rate.sleep()) { - RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + controller_frequency_trigger_ += 4; + auto & clk = *this->get_clock(); + if (controller_frequency_trigger_ >= CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD) { + RCLCPP_ERROR_THROTTLE( + get_logger(), clk, 1000, "Control loop missed its desired rate of %.4fHz", + controller_frequency_); + } else { + RCLCPP_WARN( + get_logger(), "Control loop missed its desired rate of %.4fHz", controller_frequency_); + } + } else { + controller_frequency_trigger_ -= 1; } + + // Cap the trigger counter so that it doesn't go negative or become to large + controller_frequency_trigger_ = std::min( + std::max(controller_frequency_trigger_, 0), + static_cast(CONTROLLER_FREQUENCY_TRIGGER_THRESHOLD * 1.5)); } } catch (nav2_core::PlannerException & e) { RCLCPP_ERROR(this->get_logger(), e.what()); @@ -405,6 +449,7 @@ void ControllerServer::computeControl() // TODO(orduno) #861 Handle a pending preemption and set controller name action_server_->succeeded_current(); + controller_frequency_trigger_ = 0; } void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) @@ -430,46 +475,61 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) void ControllerServer::computeAndPublishVelocity() { + geometry_msgs::msg::TwistStamped cmd_vel_2d; geometry_msgs::msg::PoseStamped pose; if (!getRobotPose(pose)) { throw nav2_core::PlannerException("Failed to obtain robot pose"); } - if (!progress_checker_->check(pose)) { - throw nav2_core::PlannerException("Failed to make progress"); - } - - nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); + if (is_safety_status_ok_) { + if (!progress_checker_->check(pose)) { + throw nav2_core::PlannerException("Failed to make progress"); + } - geometry_msgs::msg::TwistStamped cmd_vel_2d; + nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); - try { - cmd_vel_2d = - controllers_[current_controller_]->computeVelocityCommands( - pose, - nav_2d_utils::twist2Dto3D(twist), - goal_checkers_[current_goal_checker_].get()); - last_valid_cmd_time_ = now(); - } catch (nav2_core::PlannerException & e) { - if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { - RCLCPP_WARN(this->get_logger(), e.what()); - cmd_vel_2d.twist.angular.x = 0; - cmd_vel_2d.twist.angular.y = 0; - cmd_vel_2d.twist.angular.z = 0; - cmd_vel_2d.twist.linear.x = 0; - cmd_vel_2d.twist.linear.y = 0; - cmd_vel_2d.twist.linear.z = 0; - cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); - cmd_vel_2d.header.stamp = now(); - if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && - failure_tolerance_ != -1.0) - { - throw nav2_core::PlannerException("Controller patience exceeded"); + try { + cmd_vel_2d = + controllers_[current_controller_]->computeVelocityCommands( + pose, + nav_2d_utils::twist2Dto3D(twist), + goal_checkers_[current_goal_checker_].get()); + last_valid_cmd_time_ = now(); + } catch (nav2_core::PlannerException & e) { + if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { + RCLCPP_WARN(this->get_logger(), e.what()); + cmd_vel_2d.twist.angular.x = 0; + cmd_vel_2d.twist.angular.y = 0; + cmd_vel_2d.twist.angular.z = 0; + cmd_vel_2d.twist.linear.x = 0; + cmd_vel_2d.twist.linear.y = 0; + cmd_vel_2d.twist.linear.z = 0; + cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); + cmd_vel_2d.header.stamp = now(); + if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && + failure_tolerance_ != -1.0) + { + throw nav2_core::PlannerException("Controller patience exceeded"); + } + } else { + throw nav2_core::PlannerException(e.what()); } - } else { - throw nav2_core::PlannerException(e.what()); } + } else { + // Pause navigation due to safety status not OK + progress_checker_->reset(); + last_valid_cmd_time_ = now(); + cmd_vel_2d.twist.angular.x = 0; + cmd_vel_2d.twist.angular.y = 0; + cmd_vel_2d.twist.angular.z = 0; + cmd_vel_2d.twist.linear.x = 0; + cmd_vel_2d.twist.linear.y = 0; + cmd_vel_2d.twist.linear.z = 0; + cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); + cmd_vel_2d.header.stamp = now(); + // Setting the speed limit to -1 is an ugly hack to reset speed ramp in pure pursuit + controllers_[current_controller_]->setSpeedLimit(-1.0, false); } std::shared_ptr feedback = std::make_shared(); @@ -581,4 +641,9 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar } } +void ControllerServer::safetyStatusCallback(const relox_msgs::msg::SafetyStatus::SharedPtr msg) +{ + is_safety_status_ok_ = msg->status == relox_msgs::msg::SafetyStatus::OK; +} + } // namespace nav2_controller diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 9c87714fad7..5ccc9b5e819 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -52,13 +52,13 @@ install(DIRECTORY include/ DESTINATION include/ ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - set(ament_cmake_copyright_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) -endif() +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# # the following line skips the linter which checks for copyrights +# set(ament_cmake_copyright_FOUND TRUE) +# ament_lint_auto_find_test_dependencies() +# add_subdirectory(test) +# endif() ament_export_include_directories(include) ament_export_libraries(${library_name}) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 4c1a673c8af..084aed36257 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -108,13 +108,18 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + void resetSpeedRamp(); + protected: /** * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses * @param pose pose to transform * @return Path in new frame */ - nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double & distance_to_goal + ); /** * @brief Transform a pose to another frame. @@ -209,8 +214,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose_cost cost at this pose */ void applyConstraints( - const double & dist_error, const double & lookahead_dist, + const double & distance_to_goal, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & deviation_from_path, const double & pose_cost, double & linear_vel, double & sign); /** @@ -221,6 +227,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + /** + * @brief Get lookahead point + * @param lookahead_dist Optimal lookahead distance + * @param path Current global path + * @param deviation_from_path output + * @param overshoot_goal output + * @return Lookahead point + */ + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &, float &, bool &); + /** * @brief checks for the cusp position * @param pose Pose input to determine the cusp position @@ -235,6 +251,15 @@ class RegulatedPurePursuitController : public nav2_core::Controller rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; rclcpp::Clock::SharedPtr clock_; + // These should be parameters eventually + const double MINIMUM_TURNING_RADIUS = 0.5; + const double MAXIMUM_DEVIATION_FROM_PATH = 2.0; + const double MAXIMUM_ALLOWED_TIME_OFF_TRACK = 1.0; + const double MAXIMUM_OVERSHOOT = 0.2; + const double MAXIMUM_ALLOWED_TIME_OVERSHOOT = 1.0; + const double NEAR_GOAL_SLOWDOWN_DISTANCE = 4.0; + const double PROJECTION_TIME = 0.1; + double desired_linear_vel_, base_desired_linear_vel_; double lookahead_dist_; double rotate_to_heading_angular_vel_; @@ -259,11 +284,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; + std::chrono::system_clock::time_point reset_speed_ramp_ts_; + std::chrono::system_clock::time_point last_on_track_ts_; + std::chrono::system_clock::time_point last_no_overshoot_ts_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; - std::shared_ptr> - carrot_pub_; + std::shared_ptr> carrot_pub_; + std::shared_ptr> closest_pub_; std::shared_ptr> carrot_arc_pub_; }; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index ada60b6b762..a885a5ba04d 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -32,6 +32,7 @@ using std::abs; using nav2_util::declare_parameter_if_not_declared; using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT +using namespace std::chrono_literals; // NOLINT namespace nav2_regulated_pure_pursuit_controller { @@ -171,6 +172,7 @@ void RegulatedPurePursuitController::configure( global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); + closest_pub_ = node->create_publisher("closest_point", 1); carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); } @@ -183,6 +185,7 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); + closest_pub_.reset(); carrot_arc_pub_.reset(); } @@ -195,6 +198,7 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); + closest_pub_->on_activate(); carrot_arc_pub_->on_activate(); } @@ -207,6 +211,7 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); + closest_pub_->on_deactivate(); carrot_arc_pub_->on_deactivate(); } @@ -249,7 +254,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Transform path to robot base frame - auto transformed_plan = transformGlobalPlan(pose); + double distance_to_goal; + auto transformed_plan = transformGlobalPlan(pose, distance_to_goal); // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); @@ -268,6 +274,41 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); carrot_pub_->publish(createCarrotMsg(carrot_pose)); + float deviation_from_path = std::numeric_limits::max(); + bool overshoot_goal = false; + auto closes_point = getLookAheadPoint(0.0, transformed_plan, deviation_from_path, overshoot_goal); + closest_pub_->publish(createCarrotMsg(closes_point)); + + if (deviation_from_path < MAXIMUM_DEVIATION_FROM_PATH) { + last_on_track_ts_ = std::chrono::system_clock::now(); + } + + if (!overshoot_goal) { + last_no_overshoot_ts_ = std::chrono::system_clock::now(); + } + + if ( + std::chrono::duration_cast( + std::chrono::system_clock::now() - last_on_track_ts_) + .count() > MAXIMUM_ALLOWED_TIME_OFF_TRACK) { + std::ostringstream error_msg; + error_msg << "RegulatedPurePursuitController detected that the robot has deviated more than "; + error_msg << MAXIMUM_DEVIATION_FROM_PATH << " meters from its path for more than "; + error_msg << MAXIMUM_ALLOWED_TIME_OFF_TRACK << " seconds!"; + throw nav2_core::PlannerException(error_msg.str()); + } + + if ( + std::chrono::duration_cast( + std::chrono::system_clock::now() - last_no_overshoot_ts_) + .count() > MAXIMUM_ALLOWED_TIME_OVERSHOOT) { + std::ostringstream error_msg; + error_msg << "RegulatedPurePursuitController detected that the robot has overshoot its "; + error_msg << "goal by " << MAXIMUM_OVERSHOOT << " meters for more than "; + error_msg << MAXIMUM_ALLOWED_TIME_OVERSHOOT << " seconds!"; + throw nav2_core::PlannerException(error_msg.str()); + } + double linear_vel, angular_vel; // Find distance^2 to look ahead point (carrot) in robot base frame @@ -280,6 +321,10 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double curvature = 0.0; if (carrot_dist2 > 0.001) { curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + if (abs(curvature) > (1 / MINIMUM_TURNING_RADIUS)) { + const double sign = curvature < 0.0 ? -1.0 : 1.0; + curvature = (1 / MINIMUM_TURNING_RADIUS) * sign; + } } // Setting the velocity direction @@ -291,27 +336,44 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity linear_vel = desired_linear_vel_; // Make sure we're in compliance with basic constraints - double angle_to_heading; - if (shouldRotateToGoalHeading(carrot_pose)) { - double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); - rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); - } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { - rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); - } else { + // double angle_to_heading; + // if (shouldRotateToGoalHeading(carrot_pose)) { + // double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + // rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); + // } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + // rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + // } else { applyConstraints( - fabs(lookahead_dist - sqrt(carrot_dist2)), + distance_to_goal, lookahead_dist, curvature, speed, + deviation_from_path, costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity angular_vel = linear_vel * curvature; - } + // } // Collision checking on this velocity heading if (isCollisionImminent(pose, linear_vel, angular_vel)) { throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); } + // When we only have one pose left it means that we are near the end of the path + if (transformed_plan.poses.size() == 1) { + double x = transformed_plan.poses.at(0).pose.position.x; + // The transformed plan is in the robot frame so we can look where the goal is in the + // forward (x) direction. + if (x < -goal_dist_tol_) { + // If we have gone past the goal this far there is not sane to recover, instead fail + std::ostringstream error_msg; + error_msg << "RegulatedPurePursuitController detected that the robot has overshot its goal"; + throw nav2_core::PlannerException(error_msg.str()); + } else if (x < 0) { + // If we are in front of the goal we need to back up a bit + linear_vel = -min_approach_linear_velocity_; + } + } + // populate and return message geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.header = pose.header; @@ -354,19 +416,112 @@ void RegulatedPurePursuitController::rotateToHeading( geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan) +{ + float dont_care_1 = std::numeric_limits::max(); + bool dont_care_2 = false; + return getLookAheadPoint(lookahead_dist, transformed_plan, dont_care_1, dont_care_2); +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan, + float & deviation_from_path, bool & overshoot_goal) { // Find the first pose which is at a distance greater than the lookahead distance - auto goal_pose_it = std::find_if( + // -> and also in front of the robot! + auto b_pose_it = std::find_if( transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { - return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist && + ps.pose.position.x >= 0; }); // If the no pose is not far enough, take the last pose - if (goal_pose_it == transformed_plan.poses.end()) { - goal_pose_it = std::prev(transformed_plan.poses.end()); + if (b_pose_it == transformed_plan.poses.end()) { + b_pose_it = std::prev(transformed_plan.poses.end()); + // RCLCPP_INFO(logger_, "Using last pose"); + geometry_msgs::msg::PoseStamped res = *b_pose_it; + deviation_from_path = + std::sqrt(std::pow(res.pose.position.x, 2) + std::pow(res.pose.position.y, 2)); + + if (res.pose.position.x < -MAXIMUM_OVERSHOOT) { + overshoot_goal = true; + } + + // If the last pose is closer than the lookahead distance we push it forward along the x axis + // of the last pose until it is approximately one lookahead distance away from the robot. We + // do this because otherwise the robot can turn too sharply when it gets close to the goal. + tf2::Transform res_pose_tf; + tf2::fromMsg(res.pose, res_pose_tf); + tf2::Transform res_pose_tf_cpy = res_pose_tf; + // Remove the rotation from the goal pose so that x points forward + res_pose_tf_cpy.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + res_pose_tf = res_pose_tf_cpy.inverseTimes(res_pose_tf); + // Push forward in the x forward direction + res_pose_tf.getOrigin().setX( + res_pose_tf.getOrigin().x() + lookahead_dist - + KDL::sign(res.pose.position.x) * hypot(res.pose.position.x, res.pose.position.y)); + // Add back the rotation that we removed earlier + res_pose_tf = res_pose_tf_cpy * res_pose_tf; + tf2::toMsg(res_pose_tf, res.pose); + // std::cout << "last x " << res.pose.position.x << " last y " + // << res.pose.position.y << std::endl; + return res; } - return *goal_pose_it; + // Special case if the pose is the first pose in the plan + if (b_pose_it == transformed_plan.poses.begin()) { + // RCLCPP_INFO(logger_, "Using first pose"); + auto pose = *b_pose_it; + deviation_from_path = + std::sqrt(std::pow(pose.pose.position.x, 2) + std::pow(pose.pose.position.y, 2)); + // std::cout << "first x " << pose.pose.position.x << " first y " << pose.pose.position.y + // << std::endl; + return pose; + } + + // If it's not the first pose then we interpolate with the previous pose + auto a_pose_it = std::prev(b_pose_it); + + auto a_pose = *a_pose_it; + auto b_pose = *b_pose_it; + + // https://math.stackexchange.com/questions/422602/convert-two-points-to-line-eq-ax-by-c-0 + float A = a_pose.pose.position.y - b_pose.pose.position.y; + float B = b_pose.pose.position.x - a_pose.pose.position.x; + float C = a_pose.pose.position.x * b_pose.pose.position.y - + b_pose.pose.position.x * a_pose.pose.position.y; + + // https://cp-algorithms.com/geometry/circle-line-intersection.html + float d0 = fabs(C) / sqrt(A * A + B * B); + // closest point on line + float x0 = -A * C / (A * A + B * B); + float y0 = -B * C / (A * A + B * B); + + // zero or one intersection + float carrot_x = x0 + B * lookahead_dist; + float carrot_y = y0 - A * lookahead_dist; + // two intersections + if (d0 < lookahead_dist) { + float d = sqrt(lookahead_dist * lookahead_dist - C * C / (A * A + B * B)); + float m = sqrt(d * d / (A * A + B * B)); + carrot_x = x0 + B * m; + carrot_y = y0 - A * m; + // std::cout << "A * A + B * B = " << A * A + B * B << " C = " << C << " d0 = " << d0 + // << " A = " << A << " B = " << B << " d = " << d << " m = " << m << std::endl; + } else { + // std::cout << "A * A + B * B = " << A * A + B * B << " C = " << C << " d0 = " << d0 + // << " A = " << A << " B = " << B << std::endl; + } + + // std::cout << "carrot_x " << carrot_x << " carrot_y " << carrot_y << std::endl; + + geometry_msgs::msg::PoseStamped carrot_pose; + carrot_pose.header.frame_id = b_pose.header.frame_id; + carrot_pose.header.stamp = b_pose.header.stamp; + carrot_pose.pose.position.x = carrot_x; + carrot_pose.pose.position.y = carrot_y; + + deviation_from_path = sqrt(x0 * x0 + y0 * y0); + return carrot_pose; } bool RegulatedPurePursuitController::isCollisionImminent( @@ -389,26 +544,19 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.header.frame_id = arc_pts_msg.header.frame_id; pose_msg.header.stamp = arc_pts_msg.header.stamp; - const double projection_time = costmap_->getResolution() / fabs(linear_vel); - geometry_msgs::msg::Pose2D curr_pose; curr_pose.x = robot_pose.pose.position.x; curr_pose.y = robot_pose.pose.position.y; curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); int i = 1; - while (true) { - // only forward simulate within time requested - if (i * projection_time > max_allowed_time_to_collision_) { - break; - } - + while (i * PROJECTION_TIME < max_allowed_time_to_collision_) { i++; // apply velocity at curr_pose over distance - curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); - curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); - curr_pose.theta += projection_time * angular_vel; + curr_pose.x += PROJECTION_TIME * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += PROJECTION_TIME * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += PROJECTION_TIME * angular_vel; // store it for visualization pose_msg.pose.position.x = curr_pose.x; @@ -469,13 +617,20 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double } void RegulatedPurePursuitController::applyConstraints( - const double & dist_error, const double & lookahead_dist, + const double & distance_to_goal, const double & /*lookahead_dist*/, const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, + const double & deviation_from_path, const double & pose_cost, double & linear_vel, double & sign) { double curvature_vel = linear_vel; double cost_vel = linear_vel; double approach_vel = linear_vel; + double deviation_vel = linear_vel; + + // limit linear velocity by deviation from path + deviation_vel *= 0.1 + 0.9 * + std::pow( + std::clamp(1.0 - deviation_from_path / MAXIMUM_DEVIATION_FROM_PATH, 0.0, 1.0), 2); // limit the linear velocity by curvature const double radius = fabs(1.0 / curvature); @@ -499,7 +654,7 @@ void RegulatedPurePursuitController::applyConstraints( } // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed - linear_vel = std::min(cost_vel, curvature_vel); + linear_vel = std::min({cost_vel, curvature_vel, deviation_vel}); linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); // if the actual lookahead distance is shorter than requested, that means we're at the @@ -507,8 +662,8 @@ void RegulatedPurePursuitController::applyConstraints( // This expression is eq. to (1) holding time to goal, t, constant using the theoretical // lookahead distance and proposed velocity and (2) using t with the actual lookahead // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { - double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + if (distance_to_goal < NEAR_GOAL_SLOWDOWN_DISTANCE) { + double velocity_scaling = std::clamp(distance_to_goal / NEAR_GOAL_SLOWDOWN_DISTANCE, 0.0, 1.0); double unbounded_vel = approach_vel * velocity_scaling; if (unbounded_vel < min_approach_linear_velocity_) { approach_vel = min_approach_linear_velocity_; @@ -520,21 +675,50 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = std::min(linear_vel, approach_vel); } + // Speed ramp + // https://www.wolframalpha.com/input?i=plot+1%2F%281%2Bexp%28a%2Bbx%29%29+and+a%2Bbx+where+a%3D5+and+b%3D-0.7+and+x+from+-1+to+10+and+y+from+-1+to+2 + auto now = std::chrono::system_clock::now(); + auto speed_ramp_duration = now - reset_speed_ramp_ts_; + double t = static_cast( + std::chrono::duration_cast(speed_ramp_duration).count() + ) / 1.e3; + const double const_time = 5.0; // seconds to drive constant speed + const double ramp_time = 10.0; // seconds to ramp up from the constant speed to the full speed + if (t < const_time + ramp_time) { + double ramp = 0.05; // constant time + if (t > const_time) { + // ramp + ramp = ((1.0 - ramp) / ramp_time) * (t - const_time) + ramp; + } + + linear_vel *= ramp; + // RCLCPP_INFO(logger_, "t: %f, ramp: %f", t, ramp); + } + // Limit linear velocities to be valid - linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); + linear_vel = std::clamp(fabs(linear_vel), 0.02, desired_linear_vel_); linear_vel = sign * linear_vel; + + // RCLCPP_INFO( + // logger_, + // "deviation: %.3f, curvature: %.3f, distance_to_goal: %.3f, cost_vel: %.3f, curvature_vel: %.3f, deviation_vel: %.3f, desired_linear_vel: %.3f, approach_vel: %.3f, linear_vel: %.3f", + // deviation_from_path, curvature, distance_to_goal, cost_vel, curvature_vel, deviation_vel, desired_linear_vel_, approach_vel, linear_vel); } void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { global_plan_ = path; + resetSpeedRamp(); } void RegulatedPurePursuitController::setSpeedLimit( const double & speed_limit, const bool & percentage) { - if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { + if (speed_limit == -1) { + // magic value -1 is the nav2 controller server telling us to reset the speed ramp + resetSpeedRamp(); + } else if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value desired_linear_vel_ = base_desired_linear_vel_; } else { @@ -548,8 +732,15 @@ void RegulatedPurePursuitController::setSpeedLimit( } } +void RegulatedPurePursuitController::resetSpeedRamp() +{ + reset_speed_ramp_ts_ = std::chrono::system_clock::now(); +} + nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) + const geometry_msgs::msg::PoseStamped & pose, + double & distance_to_goal +) { if (global_plan_.poses.empty()) { throw nav2_core::PlannerException("Received plan with zero length"); @@ -561,26 +752,40 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); } + // Calculate the distance from the robot to the end of the global plan + distance_to_goal = euclidean_distance(robot_pose, global_plan_.poses.back()); + // We'll discard points on the plan that are outside the local costmap nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; // First find the closest pose on the path to the robot + auto last_pose_it = std::min(global_plan_.poses.begin() + 5, global_plan_.poses.end()); auto transformation_begin = nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), global_plan_.poses.end(), + global_plan_.poses.begin(), last_pose_it, [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { return euclidean_distance(robot_pose, ps); }); // Find points definitely outside of the costmap so we won't transform them. auto transformation_end = std::find_if( - transformation_begin, end(global_plan_.poses), + transformation_begin + 2, end(global_plan_.poses), [&](const auto & global_plan_pose) { return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; }); + // Add the next outside the costmap + if (transformation_end < end(global_plan_.poses)) { + transformation_end++; + } + + // Add the previous outside the costmap + if (transformation_begin > begin(global_plan_.poses)) { + transformation_begin--; + } + // Lambda to transform a PoseStamped from global frame to local auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 6da37d4c20b..d1036125c34 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -117,12 +117,12 @@ install(DIRECTORY include/ DESTINATION include/ ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) -endif() +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +# find_package(ament_cmake_gtest REQUIRED) +# add_subdirectory(test) +# endif() ament_export_include_directories(include) ament_export_libraries(${library_name} ${library_name}_2d) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 39d5788b97d..04ef274c871 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -137,7 +137,9 @@ class AStarAlgorithm void setGoal( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3); + const unsigned int & dim_3, + const double & cmx, + const double & cmy); /** * @brief Set the starting pose for planning, as a node index @@ -148,7 +150,9 @@ class AStarAlgorithm void setStart( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3); + const unsigned int & dim_3, + const double & cmx, + const double & cmy); /** * @brief Get maximum number of iterations to plan diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index 0c58fbfc012..240152dd21e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -46,6 +46,17 @@ inline geometry_msgs::msg::Pose getWorldCoords( return msg; } +inline geometry_msgs::msg::Pose getMapCoords( + const float & wx, const float & wy, const nav2_costmap_2d::Costmap2D * costmap) +{ + geometry_msgs::msg::Pose msg; + msg.position.x = + (wx - static_cast(costmap->getOriginX())) / costmap->getResolution() - 0.5; + msg.position.y = + (wy - static_cast(costmap->getOriginY())) / costmap->getResolution() - 0.5; + return msg; +} + /** * @brief Create quaternion from A* coord bins * @param theta continuous bin coordinates angle diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 11d1976dae3..8e460ce6d87 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -121,7 +121,9 @@ template<> void AStarAlgorithm::setStart( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const double & cmx, + const double & cmy) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); @@ -133,13 +135,15 @@ template void AStarAlgorithm::setStart( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const double & cmx, + const double & cmy) { _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); _start->setPose( Coordinates( - static_cast(mx), - static_cast(my), + static_cast(cmx), + static_cast(cmy), static_cast(dim_3))); } @@ -147,7 +151,9 @@ template<> void AStarAlgorithm::setGoal( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const double & cmx, + const double & cmy) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); @@ -161,13 +167,15 @@ template void AStarAlgorithm::setGoal( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const double & cmx, + const double & cmy) { _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); typename NodeT::Coordinates goal_coords( - static_cast(mx), - static_cast(my), + static_cast(cmx), + static_cast(cmy), static_cast(dim_3)); if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { @@ -329,6 +337,10 @@ bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path current_node = current_node->parent; } + path.push_back( + Node2D::getCoords( + current_node->getIndex(), getSizeX(), getSizeDim3())); + return path.size() > 0; } @@ -346,6 +358,8 @@ bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) current_node = current_node->parent; } + path.push_back(current_node->pose); + return path.size() > 0; } diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index bf1d78721cc..842e2e137b6 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -224,11 +224,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Set starting point unsigned int mx_start, my_start, mx_goal, my_goal; costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); - _a_star->setStart(mx_start, my_start, 0); + _a_star->setStart(mx_start, my_start, 0, 0, 0); // Set goal point costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); - _a_star->setGoal(mx_goal, my_goal, 0); + _a_star->setGoal(mx_goal, my_goal, 0, 0, 0); // Setup message nav_msgs::msg::Path plan; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 189ba66c6bc..b4a755dc0bb 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -271,7 +271,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( orientation_bin -= static_cast(_angle_quantizations); } unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setStart(mx, my, orientation_bin_id); + + auto mapPose = getMapCoords(start.pose.position.x, start.pose.position.y, costmap); + + _a_star->setStart(mx, my, orientation_bin_id, mapPose.position.x, mapPose.position.y); // Set goal point, in A* bin search coordinates costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); @@ -284,7 +287,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( orientation_bin -= static_cast(_angle_quantizations); } orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setGoal(mx, my, orientation_bin_id); + + mapPose = getMapCoords(goal.pose.position.x, goal.pose.position.y, costmap); + + _a_star->setGoal(mx, my, orientation_bin_id, mapPose.position.x, mapPose.position.y); // Setup message nav_msgs::msg::Path plan; From 3ec119ab874a874c0601203104d17049c643f51c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20=C3=96rjehag?= Date: Wed, 15 May 2024 09:00:02 +0200 Subject: [PATCH 2/2] Copy smac planner from ros-2-main --- nav2_smac_planner/CMakeLists.txt | 42 +- nav2_smac_planner/README.md | 92 ++- .../include/nav2_smac_planner/a_star.hpp | 95 +-- .../nav2_smac_planner/analytic_expansion.hpp | 122 ++++ .../nav2_smac_planner/collision_checker.hpp | 172 ++--- .../include/nav2_smac_planner/constants.hpp | 3 +- .../nav2_smac_planner/costmap_downsampler.hpp | 17 +- .../include/nav2_smac_planner/node_basic.hpp | 49 +- .../include/nav2_smac_planner/node_hybrid.hpp | 200 +++--- .../nav2_smac_planner/smac_planner_hybrid.hpp | 34 +- .../include/nav2_smac_planner/smoother.hpp | 342 +++++----- .../include/nav2_smac_planner/types.hpp | 87 ++- .../include/nav2_smac_planner/utils.hpp | 79 ++- nav2_smac_planner/package.xml | 9 +- nav2_smac_planner/smac_plugin_hybrid.xml | 5 + nav2_smac_planner/src/a_star.cpp | 625 +++++++----------- nav2_smac_planner/src/analytic_expansion.cpp | 243 +++++++ nav2_smac_planner/src/collision_checker.cpp | 168 +++++ nav2_smac_planner/src/costmap_downsampler.cpp | 46 +- nav2_smac_planner/src/node_basic.cpp | 41 ++ nav2_smac_planner/src/node_hybrid.cpp | 369 ++++++----- nav2_smac_planner/src/smac_planner_hybrid.cpp | 257 ++++--- nav2_smac_planner/src/smoother.cpp | 486 ++++++++++++++ 23 files changed, 2255 insertions(+), 1328 deletions(-) create mode 100644 nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp create mode 100644 nav2_smac_planner/smac_plugin_hybrid.xml create mode 100644 nav2_smac_planner/src/analytic_expansion.cpp create mode 100644 nav2_smac_planner/src/collision_checker.cpp create mode 100644 nav2_smac_planner/src/node_basic.cpp create mode 100644 nav2_smac_planner/src/smoother.cpp diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index d1036125c34..b4a850f69a5 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -21,8 +21,10 @@ find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(angles REQUIRED) find_package(ompl REQUIRED) find_package(OpenMP REQUIRED) +find_package(relox_costmap_2d REQUIRED) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -66,16 +68,21 @@ set(dependencies nav2_costmap_2d nav2_core pluginlib + angles eigen3_cmake_module + relox_costmap_2d ) # Hybrid plugin add_library(${library_name} SHARED src/smac_planner_hybrid.cpp src/a_star.cpp + src/collision_checker.cpp + src/smoother.cpp + src/analytic_expansion.cpp src/node_hybrid.cpp src/costmap_downsampler.cpp - src/node_2d.cpp + src/node_basic.cpp ) target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) @@ -85,29 +92,9 @@ ament_target_dependencies(${library_name} ${dependencies} ) -# 2D plugin -add_library(${library_name}_2d SHARED - src/smac_planner_2d.cpp - src/a_star.cpp - src/node_hybrid.cpp - src/costmap_downsampler.cpp - src/node_2d.cpp -) - -target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES}) -target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) +pluginlib_export_plugin_description_file(nav2_core smac_plugin_hybrid.xml) -ament_target_dependencies(${library_name}_2d - ${dependencies} -) - -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -target_compile_definitions(${library_name}_2d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file(nav2_core smac_plugin.xml) -pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) - -install(TARGETS ${library_name} ${library_name}_2d +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} @@ -117,14 +104,7 @@ install(DIRECTORY include/ DESTINATION include/ ) -# if(BUILD_TESTING) -# find_package(ament_lint_auto REQUIRED) -# ament_lint_auto_find_test_dependencies() -# find_package(ament_cmake_gtest REQUIRED) -# add_subdirectory(test) -# endif() - ament_export_include_directories(include) -ament_export_libraries(${library_name} ${library_name}_2d) +ament_export_libraries(${library_name}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5d36fb47e4f..086403edf05 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -1,31 +1,43 @@ # Smac Planner -The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 2 distinct plugins: -- `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (ackermann and car models). +The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins: +- `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models). + - `SmacPlannerLattice`: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional models. - `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. It also introduces the following basic building blocks: - `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. -- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A*, SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. +- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Additional template for planning also could be made available using it. - `CollisionChecker`: Collision check based on a robot's radius or footprint. -- `Smoother`: A path smoother to smooth out 2D, Hybrid-A\* paths. +- `Smoother`: A simple path smoother to smooth out 2D, Hybrid-A\*, and State Lattice paths. We have users reporting using this on: - Delivery robots - Industrial robots +- Vertical farming +- Solar farms + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. ## Introduction -The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner. These plugins are also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. +The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **legged, cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner. We support **non-circular, arbitrary shaped, any model vehicles** using the `SmacPlannerLattice` plugin which implements a State Lattice planner. It contains control sets and generators for ackermann, legged, differential drive and omnidirectional vehicles, but you may provide your own for another robot type or to have different planning behaviors. The last two plugins are also useful for curvature constrained or kinematically feasible planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. + +The `SmacPlannerHybrid` implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), with modifications to the heuristic, traversal functions to increase path quality without needing expensive optimization-based smoothing. -The `SmacPlannerHybrid` implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, gradient descent smoothing, analytic expansions and hueristic functions. +The `SmacPlannerLattice` implements the State Lattice planner. While we do not implement it precisely the same way as [Optimal, Smooth, Nonholonomic MobileRobot Motion Planning in State Lattices](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2007_1/pivtoraiko_mihail_2007_1.pdf) (with control sets found using [Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf)), it is sufficiently similar it may be used as a good reference. Additional optimizations for on-approach analytic expansions and improved heuristic functions were used, largely matching those of Hybrid-A\* to allow them to share these optimized implementations to drive search towards the goal, faster. In summary... The `SmacPlannerHybrid` is designed to work with: - Ackermann, car, and car-like robots - High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds) -- Arbitrary shaped, non-circular differential or omnidirectional robots requiring SE2 collision checking with constrained curvatures +- Arbitrary shaped, non-circular differential or omnidirectional robots requiring kinematically feasible planning with SE2 collision checking +- Legged robots + +The `SmacPlannerLattice` is designed to work with: +- Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain +- Flexibility to use other robot model types or with provided non-circular differental, ackermann, and omni support The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots @@ -33,46 +45,50 @@ The `SmacPlanner2D` is designed to work with: ## Features -We further improve in the following ways: +We further improve on Hybrid-A\* in the following ways: - Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). - Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster). - Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth). -- Gradient descent smoother +- Gradient-descent, basic but fast smoother - Faster planning than original paper by highly optimizing the template A\* algorithm. -- Faster planning via precomputed heuristic, motion primitive, and other values. +- Faster planning via custom precomputed heuristic, motion primitive, and other functions. - Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing. - Closest path on approach within tolerance if exact path cannot be found or in invalid space. - Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added. - High unit and integration test coverage, doxygen documentation. - Uses modern C++14 language features and individual components are easily reusable. -- Speed optimizations: no data structure graph lookups in main loop, near-zero copy main loop, dynamically generated graph and dynamic programming-based obstacle heuristic, optional recomputation of heuristics for subsiquent planning requests of the same goal, etc. +- Speed optimizations: no data structure graph lookups in main loop, near-zero copy main loop, dynamically generated graph and dynamic programming-based obstacle heuristic, optional recomputation of heuristics for subsequent planning requests of the same goal, etc. - Templated Nodes and A\* implementation to support additional robot extensions. -- Selective re-evaluation of the obstacle heuristic per goal/map or each iteration, which can speed up subsiquent replanning 20x or more. +- Selective re-evaluation of the obstacle heuristic per goal/map or each iteration, which can speed up subsequent replanning 20x or more. -All of these features (multi-resolution, models, smoother, etc) are also available in the `SmacPlanner2D` plugin. +Most of these features (multi-resolution, models, smoother, etc) are also available in the `SmacPlanner2D` and `SmacPlannerLattice` plugins. -The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. Though the `SmacPlanner2D` is grid-based, any reasonable local trajectory planner - including those supported by Nav2 - will not have any issue with grid-based plans. +The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. -Note: In prior releases, a CG smoother largely implementing the original Hybrid-A\* paper's. However, this smoother failed to consistently provide useful results, took too much compute time, and was deprecated. While smoothing a path 95% of the time seems like a "good" solution, we need something far more reliable for practical use. Since we are working with mobile robots and not autonomous cars at 60 mph, we can take some different liberties in smoothing knowing that our local trajectory planners are pretty smart. If you are looking for it, it now lives in the `test/` directory in a depreciated folder. This smoother has been replaced by a B-Spline inspired solution which is faster, far more consistent, and simpler to understand. While this smoother is **not** cost-aware, we have added cost-aware penalty functions in the planners themselves to push the plans away from high-cost spaces and we do check for validity of smoothed sections to ensure feasibility. +Note: In prior releases, a CG smoother largely implementing the original Hybrid-A\* paper's. However, this smoother failed to consistently provide useful results, took too much compute time, and was deprecated. While smoothing a path 95% of the time seems like a "good" solution, we need something far more reliable for practical use. Since we are working with mobile robots and not autonomous cars at 60 mph, we can take some different liberties in smoothing knowing that our local trajectory planners are pretty smart. If you are looking for it, it now lives in the new Smoothing Server as the Cost-aware smoother. This smoother has been replaced by a simpler optimization inspired solution which is faster, far more consistent, and simpler to understand. While this smoother is **not** cost-aware, we have added cost-aware penalty functions in the planners themselves to push the plans away from high-cost spaces and we do check for validity of smoothed sections to ensure feasibility. It will through terminate when paths become in collision with the environment. If you would like to use this smoother, however, it is available in the smoother server, though it will take some additional compute time. ## Metrics The original Hybrid-A\* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations: - **2-20ms** for planning across 147,456 (1.4x larger) cell maps with 72 angular bins. -- **30-120ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins. +- **30-200ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins. -For example, the following path (roughly 85 meters) path took 33ms to compute. +An example of the 3 planners can be seen below, planning a roughly 75 m path. +- 2D A* computed the path in 243ms (Panel 1) +- Hybrid-A* computed the path in 144ms (Panel 2) +- State Lattice computed the path in 113ms (Panel 3) +- For reference: NavFn compute the path in 146ms, including some nasty path discontinuity artifacts -![alt text](test/path.png) +![alt text](test/3planners.png) ## Design The basic design centralizes a templated A\* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, `NodeT`, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A\* implementation that can be used to traverse any type of graph as long as a node template can be created for it. -We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother smooths it out on turns. We also provide a Hybrid A\* node template (`NodeHybrid`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. +We provide 3 nodes by default currently. The 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods. We also provide a Hybrid A\* node template (`NodeHybrid`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. We also provide the Lattice (`NodeLattice`) node for state lattice planning making use of the wider range of velocity options available to differential and omnidirectional robots. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. -In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the `SmacPlannerHybrid` plugin, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. +In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the `SmacPlannerHybrid` and `SmacPlannerLattice` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. @@ -98,21 +114,25 @@ planner_server: motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) - minimum_turning_radius: 0.40 # For Hybrid nodes: minimum turning radius in m of path / vehicle - angle_quantization_bins: 64 # For Hybrid/Lattice nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.15 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 - non_straight_penalty: 1.50 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 1.7 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.20 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 + rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. lookup_table_size: 20 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. + smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. smoother: max_iterations: 1000 w_smooth: 0.3 w_data: 0.2 tolerance: 1e-10 + do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further ``` ## Topics @@ -125,7 +145,7 @@ planner_server: ## Install ``` -sudo apt-get install ros--smac-planner +sudo apt-get install ros--nav2-smac-planner ``` ## Etc (Important Side Notes) @@ -140,7 +160,7 @@ This habit actually results in paths produced by NavFn, Global Planner, and now So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. -### Hybrid-A* Turning Radius' +### Hybrid-A* and State Lattice Turning Radius' A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in. @@ -150,12 +170,26 @@ By default, `0.4m` is the setting which I think is "reasonable" for the smaller ### Costmap Resolutions -We provide for the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms. +We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms. I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. +### Penalty Function Tuning + +The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. + +**However**, due to the nature of the State Lattice planner being able to use any number of custom generated minimum control sets, this planner may require more tuning to get good behavior. The defaults for State Lattice were generated using the 5cm Ackermann files you can find in this package as initial examples. After a change in formulation for the Hybrid-A* planner, the default of change penalty off seems to produce good results, but please tune to your application need and run-time speed requirements. + +When tuning, the "reasonable" range for each penalty is listed below. While you may obviously tune outside of these ranges, I've found that they offer a good trade-off and outside of these ranges behaviors get suboptimal quickly. +- Cost: 1.7 - 6.0 +- Non-Straight: 1.0 - 1.3 +- Change: 0.0 - 0.3 +- Reverse: 1.3 - 5.0 + +Note that change penalty must be greater than 0.0. The Non-staight, reverse, and cost penalties must be greater than 1.0, strictly. + ### No path found for clearly valid goals or long compute times Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of. diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 04ef274c871..0421d80849c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -16,30 +16,29 @@ #ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_ #define NAV2_SMAC_PLANNER__A_STAR_HPP_ -#include #include -#include #include #include +#include #include -#include "Eigen/Core" +#include +#include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" - +#include "nav2_smac_planner/analytic_expansion.hpp" +#include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/constants.hpp" namespace nav2_smac_planner { - /** * @class nav2_smac_planner::AStarAlgorithm * @brief An A* implementation for planning in a costmap. Templated based on the Node type. */ -template +template class AStarAlgorithm { public: @@ -50,28 +49,7 @@ class AStarAlgorithm typedef typename NodeT::Coordinates Coordinates; typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; - typedef std::function NodeGetter; - - /** - * @struct nav2_smac_planner::AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - */ - - struct AnalyticExpansionNode - { - AnalyticExpansionNode( - NodePtr & node_in, - Coordinates & initial_coords_in, - Coordinates & proposed_coords_in) - : node(node_in), initial_coords(initial_coords_in), proposed_coords(proposed_coords_in) - {} - - NodePtr node; - Coordinates initial_coords; - Coordinates proposed_coords; - }; - - typedef std::vector AnalyticExpansionNodes; + typedef std::function NodeGetter; /** * @struct nav2_smac_planner::NodeComparator @@ -105,12 +83,12 @@ class AStarAlgorithm * @param max_on_approach_iterations Maximum number of iterations before returning a valid * path once within thresholds to refine path * comes at more compute time but smoother paths. + * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns + * false after this timeout */ void initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const float & lookup_table_size, + const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const double & max_planning_time, const float & lookup_table_size, const unsigned int & dim_3_size); /** @@ -135,11 +113,8 @@ class AStarAlgorithm * @param dim_3 The node dim_3 index of the goal */ void setGoal( - const unsigned int & mx, - const unsigned int & my, - const unsigned int & dim_3, - const double & cmx, - const double & cmy); + const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3, + const double & cmx, const double & cmy, const double & cdim_3); /** * @brief Set the starting pose for planning, as a node index @@ -148,11 +123,8 @@ class AStarAlgorithm * @param dim_3 The node dim_3 index of the goal */ void setStart( - const unsigned int & mx, - const unsigned int & my, - const unsigned int & dim_3, - const double & cmx, - const double & cmy); + const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3, + const double & cmx, const double & cmy, const double & cdim_3); /** * @brief Get maximum number of iterations to plan @@ -230,14 +202,6 @@ class AStarAlgorithm */ inline bool isGoal(NodePtr & node); - /** - * @brief Set the starting pose for planning, as a node index - * @param node Node pointer to the goal node to backtrace - * @param path Reference to a vector of indicies of generated path - * @return whether the path was able to be backtraced - */ - bool backtracePath(NodePtr node, CoordinateVector & path); - /** * @brief Get cost of heuristic of node * @param node Node index current @@ -262,34 +226,12 @@ class AStarAlgorithm */ inline void clearGraph(); - /** - * @brief Attempt an analytic path completion - * @return Node pointer reference to goal node if successful, else - * return nullptr - */ - NodePtr tryAnalyticExpansion( - const NodePtr & current_node, - const NodeGetter & getter, int & iterations, int & best_cost); - - /** - * @brief Perform an analytic path expansion to the goal - * @param node The node to start the analytic path from - * @param getter The function object that gets valid nodes from the graph - * @return A set of analytically expanded nodes to the goal from current node, if possible - */ - AnalyticExpansionNodes getAnalyticPath(const NodePtr & node, const NodeGetter & getter); - - /** - * @brief Takes final analytic expansion and appends to current expanded node - * @param node The node to start the analytic path from - * @param expanded_nodes Expanded nodes to append to end of current search path - * @return Node pointer to goal node if successful, else return nullptr - */ - NodePtr setAnalyticPath(const NodePtr & node, const AnalyticExpansionNodes & expanded_nodes); + int _timing_interval = 5000; bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; + double _max_planning_time; float _tolerance; unsigned int _x_size; unsigned int _y_size; @@ -308,6 +250,7 @@ class AStarAlgorithm GridCollisionChecker * _collision_checker; nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr> _expander; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp new file mode 100644 index 00000000000..896a0df34a6 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -0,0 +1,122 @@ +// 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__ANALYTIC_EXPANSION_HPP_ +#define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ + +#include +#include +#include +#include + +#include "nav2_smac_planner/constants.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/types.hpp" + +namespace nav2_smac_planner +{ +template +class AnalyticExpansion +{ +public: + typedef NodeT * NodePtr; + typedef typename NodeT::Coordinates Coordinates; + typedef std::function NodeGetter; + + /** + * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + */ + struct AnalyticExpansionNode + { + AnalyticExpansionNode( + NodePtr & node_in, Coordinates & initial_coords_in, Coordinates & proposed_coords_in) + : node(node_in), initial_coords(initial_coords_in), proposed_coords(proposed_coords_in) + { + } + + NodePtr node; + Coordinates initial_coords; + Coordinates proposed_coords; + }; + + typedef std::vector AnalyticExpansionNodes; + + /** + * @brief Constructor for analytic expansion object + */ + AnalyticExpansion( + const MotionModel & motion_model, const SearchInfo & search_info, const bool & traverse_unknown, + const unsigned int & dim_3_size); + + /** + * @brief Sets the collision checker and costmap to use in expansion validation + * @param collision_checker Collision checker to use + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Attempt an analytic path completion + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param getter Gets a node at a set of coordinates + * @param iterations Iterations to run over + * @param best_cost Best heuristic cost to propertionally expand more closer to the goal + * @return Node pointer reference to goal node if successful, else + * return nullptr + */ + NodePtr tryAnalyticExpansion( + const NodePtr & current_node, const NodePtr & goal_node, const NodeGetter & getter, + int & iterations, int & best_cost); + + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @return A set of analytically expanded nodes to the goal from current node, if possible + */ + AnalyticExpansionNodes getAnalyticPath( + const NodePtr & node, const NodePtr & goal, const NodeGetter & getter); + + /** + * @brief Takes final analytic expansion and appends to current expanded node + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param expanded_nodes Expanded nodes to append to end of current search path + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr setAnalyticPath( + const NodePtr & node, const NodePtr & goal, const AnalyticExpansionNodes & expanded_nodes); + + /** + * @brief Takes an expanded nodes to clean up, if necessary, of any state + * information that may be poluting it from a prior search iteration + * @param expanded_nodes Expanded node to clean up from search + */ + void cleanNode(const NodePtr & nodes); + +protected: + MotionModel _motion_model; + SearchInfo _search_info; + bool _traverse_unknown; + unsigned int _dim_3_size; + GridCollisionChecker * _collision_checker; + std::list> _detached_nodes; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index c38aeff1013..0cb5dfbf327 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. #include + #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" @@ -20,28 +21,33 @@ namespace nav2_smac_planner { - /** * @class nav2_smac_planner::GridCollisionChecker * @brief A costmap grid collision checker */ class GridCollisionChecker - : public nav2_costmap_2d::FootprintCollisionChecker +: public nav2_costmap_2d::FootprintCollisionChecker { public: /** * @brief A constructor for nav2_smac_planner::GridCollisionChecker + * for use when regular bin intervals are appropriate * @param costmap The costmap to collision check against * @param num_quantizations The number of quantizations to precompute footprint * orientations for to speed up collision checking */ - GridCollisionChecker( - nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations) - : FootprintCollisionChecker(costmap), - num_quantizations_(num_quantizations) - { - } + GridCollisionChecker(nav2_costmap_2d::Costmap2D * costmap, unsigned int num_quantizations); + + /** + * @brief A constructor for nav2_smac_planner::GridCollisionChecker + * for use when irregular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param angles The vector of possible angle bins to precompute for + * orientations for to speed up collision checking, in radians + */ + // GridCollisionChecker( + // nav2_costmap_2d::Costmap2D * costmap, + // std::vector & angles); /** * @brief Set the footprint to use with collision checker @@ -49,121 +55,19 @@ class GridCollisionChecker * @param radius Whether or not the footprint is a circle and use radius collision checking */ void setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_inscribed_cost) - { - possible_inscribed_cost_ = possible_inscribed_cost; - footprint_is_radius_ = radius; - - // Use radius, no caching required - if (radius) { - return; - } - - // No change, no updates required - if (footprint == unoriented_footprint_) { - return; - } - - bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); - oriented_footprints_.reserve(num_quantizations_); - double sin_th, cos_th; - geometry_msgs::msg::Point new_pt; - const unsigned int footprint_size = footprint.size(); - - // Precompute the orientation bins for checking to use - for (unsigned int i = 0; i != num_quantizations_; i++) { - sin_th = sin(i * bin_size_); - cos_th = cos(i * bin_size_); - nav2_costmap_2d::Footprint oriented_footprint; - oriented_footprint.reserve(footprint_size); - - for (unsigned int j = 0; j < footprint_size; j++) { - new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - oriented_footprint.push_back(new_pt); - } - - oriented_footprints_.push_back(oriented_footprint); - } - - unoriented_footprint_ = footprint; - } + const nav2_costmap_2d::Footprint & footprint, const bool & radius, + const double & possible_inscribed_cost); /** * @brief Check if in collision with costmap and footprint at pose * @param x X coordinate of pose to check against * @param y Y coordinate of pose to check against - * @param theta Angle of pose to check against + * @param theta Angle bin number of pose to check against (NOT radians) * @param traverse_unknown Whether or not to traverse in unknown space * @return boolean if in collision or not. */ bool inCollision( - const float & x, - const float & y, - const float & theta, - const bool & traverse_unknown) - { - // Assumes setFootprint already set - double wx, wy; - costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); - - if (!footprint_is_radius_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); - - if (footprint_cost_ < possible_inscribed_cost_) { - return false; - } - - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { - return true; - } - - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { - return true; - } - - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - int angle_bin = theta / bin_size_; - geometry_msgs::msg::Point new_pt; - const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; - nav2_costmap_2d::Footprint current_footprint; - current_footprint.reserve(oriented_footprint.size()); - for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { - new_pt.x = wx + oriented_footprint[i].x; - new_pt.y = wy + oriented_footprint[i].y; - current_footprint.push_back(new_pt); - } - - footprint_cost_ = footprintCost(current_footprint); - - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); - - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; - } - } + const float & x, const float & y, const float & theta, const bool & traverse_unknown); /** * @brief Check if in collision with costmap and footprint at pose @@ -171,36 +75,36 @@ class GridCollisionChecker * @param traverse_unknown Whether or not to traverse in unknown space * @return boolean if in collision or not. */ - bool inCollision( - const unsigned int & i, - const bool & traverse_unknown) - { - footprint_cost_ = costmap_->getCost(i); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; - } + bool inCollision(const unsigned int & i, const bool & traverse_unknown); /** * @brief Get cost at footprint pose in costmap * @return the cost at the pose in costmap */ - float getCost() - { - // Assumes inCollision called prior - return static_cast(footprint_cost_); - } + float getCost(); + + /** + * @brief Get the angles of the precomputed footprint orientations + * @return the ordered vector of angles corresponding to footprints + */ + std::vector & getPrecomputedAngles() { return angles_; } + +private: + /** + * @brief Check if value outside the range + * @param min Minimum value of the range + * @param max Maximum value of the range + * @param value the value to check if it is within the range + * @return boolean if in range or not + */ + bool outsideRange(const unsigned int & max, const float & value); protected: std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; double footprint_cost_; bool footprint_is_radius_; - unsigned int num_quantizations_; - double bin_size_; + std::vector angles_; double possible_inscribed_cost_{-1}; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 8bd0db55754..3330363bcb9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -19,8 +19,7 @@ namespace nav2_smac_planner { -enum class MotionModel -{ +enum class MotionModel { UNKNOWN = 0, VON_NEUMANN = 1, MOORE = 2, diff --git a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp index 95d12a7f02e..8c708b5c547 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp @@ -16,15 +16,14 @@ #define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ #include -#include #include +#include #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_smac_planner/constants.hpp" namespace nav2_smac_planner { - /** * @class nav2_smac_planner::CostmapDownsampler * @brief A costmap downsampler for more efficient path planning @@ -49,13 +48,12 @@ class CostmapDownsampler * @param topic_name The name of the topic to publish the downsampled costmap * @param costmap The costmap we want to downsample * @param downsampling_factor Multiplier for the costmap resolution + * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling */ void on_configure( - const nav2_util::LifecycleNode::WeakPtr & node, - const std::string & global_frame, - const std::string & topic_name, - nav2_costmap_2d::Costmap2D * const costmap, - const unsigned int & downsampling_factor); + const nav2_util::LifecycleNode::WeakPtr & node, const std::string & global_frame, + const std::string & topic_name, nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, const bool & use_min_cost_neighbor = false); /** * @brief Activate the publisher of the downsampled costmap @@ -95,15 +93,14 @@ class CostmapDownsampler * @param new_mx The X-coordinate of the cell in the new costmap * @param new_my The Y-coordinate of the cell in the new costmap */ - void setCostOfCell( - const unsigned int & new_mx, - const unsigned int & new_my); + void setCostOfCell(const unsigned int & new_mx, const unsigned int & new_my); unsigned int _size_x; unsigned int _size_y; unsigned int _downsampled_size_x; unsigned int _downsampled_size_y; unsigned int _downsampling_factor; + bool _use_min_cost_neighbor; float _downsampled_resolution; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _downsampled_costmap; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 0b69771ef45..31b02129678 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -16,31 +16,30 @@ #define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ #include -#include + #include -#include #include -#include +#include +#include #include +#include #include -#include - -#include "ompl/base/StateSpace.h" +#include +#include "nav2_smac_planner/collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/collision_checker.hpp" +#include "ompl/base/StateSpace.h" namespace nav2_smac_planner { - /** * @class nav2_smac_planner::NodeBasic * @brief NodeBasic implementation for priority queue insertion */ -template +template class NodeBasic { public: @@ -49,20 +48,30 @@ class NodeBasic * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ - explicit NodeBasic(const unsigned int index) - : index(index), - graph_node_ptr(nullptr) - { - } + explicit NodeBasic(const unsigned int index) : index(index), graph_node_ptr(nullptr) {} + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeT. + * @param node NodeT ptr to populate metadata into NodeBasic + */ + void populateSearchNode(NodeT *& node); + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeTs. + * @param node Search node (basic) object to initialize internal node + * with state + */ + void processSearchNode(); - typename NodeT::Coordinates pose; // Used by NodeHybrid + typename NodeT::Coordinates pose; // Used by NodeHybrid and NodeLattice NodeT * graph_node_ptr; - unsigned int index; + MotionPrimitive * prim_ptr; // Used by NodeLattice + unsigned int index, motion_index; + bool backward; }; -template class NodeBasic; -template class NodeBasic; - } // namespace nav2_smac_planner #endif // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index b2e31c0772e..a024252b51b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -16,57 +16,37 @@ #define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ #include -#include + #include -#include #include -#include +#include +#include #include +#include #include -#include - -#include "ompl/base/StateSpace.h" +#include -#include "nav2_smac_planner/constants.hpp" -#include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" +#include "nav2_smac_planner/types.hpp" +#include "ompl/base/StateSpace.h" namespace nav2_smac_planner { - typedef std::vector LookupTable; typedef std::pair TrigValues; -// Need seperate pose struct for motion table operations - -/** - * @struct nav2_smac_planner::MotionPose - * @brief A struct for poses in motion primitives - */ -struct MotionPose +typedef std::pair ObstacleHeuristicElement; +struct ObstacleHeuristicComparator { - /** - * @brief A constructor for nav2_smac_planner::MotionPose - */ - MotionPose() {} - - /** - * @brief A constructor for nav2_smac_planner::MotionPose - * @param x X pose - * @param y Y pose - * @param theta Angle of pose - */ - MotionPose(const float & x, const float & y, const float & theta) - : _x(x), _y(y), _theta(theta) - {} - - float _x; - float _y; - float _theta; + bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const + { + return a.first > b.first; + } }; -typedef std::vector MotionPoses; +typedef std::vector ObstacleHeuristicQueue; // Must forward declare class NodeHybrid; @@ -90,9 +70,7 @@ struct HybridMotionTable * @param search_info Parameters for searching */ void initDubin( - unsigned int & size_x_in, - unsigned int & size_y_in, - unsigned int & angle_quantization_in, + unsigned int & size_x_in, unsigned int & size_y_in, unsigned int & angle_quantization_in, SearchInfo & search_info); /** @@ -103,9 +81,7 @@ struct HybridMotionTable * @param search_info Parameters for searching */ void initReedsShepp( - unsigned int & size_x_in, - unsigned int & size_y_in, - unsigned int & angle_quantization_in, + unsigned int & size_x_in, unsigned int & size_y_in, unsigned int & angle_quantization_in, SearchInfo & search_info); /** @@ -115,6 +91,20 @@ struct HybridMotionTable */ MotionPoses getProjections(const NodeHybrid * node); + /** + * @brief Get the angular bin to use from a raw orientation + * @param theta Angle in radians + * @return bin index of closest angle to request + */ + unsigned int getClosestAngularBin(const double & theta); + + /** + * @brief Get the raw orientation from an angular bin + * @param bin_idx Index of the bin + * @return Raw orientation in radians + */ + float getAngleFromBin(const unsigned int & bin_idx); + MotionModel motion_model = MotionModel::UNKNOWN; MotionPoses projections; unsigned int size_x; @@ -126,6 +116,7 @@ struct HybridMotionTable float non_straight_penalty; float cost_penalty; float reverse_penalty; + float travel_distance_reward; ompl::base::StateSpacePtr state_space; std::vector> delta_xs; std::vector> delta_ys; @@ -162,17 +153,15 @@ class NodeHybrid */ Coordinates(const float & x_in, const float & y_in, const float & theta_in) : x(x_in), y(y_in), theta(theta_in) - {} + { + } inline bool operator==(const Coordinates & rhs) { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) - { - return !(*this == rhs); - } + inline bool operator!=(const Coordinates & rhs) { return !(*this == rhs); } float x, y, theta; }; @@ -195,19 +184,17 @@ class NodeHybrid * @param NodeHybrid right hand side node reference * @return If cell indicies are equal */ - bool operator==(const NodeHybrid & rhs) - { - return this->_index == rhs._index; - } + bool operator==(const NodeHybrid & rhs) { return this->_index == rhs._index; } /** * @brief setting continuous coordinate search poses (in partial-cells) * @param Pose pose */ - inline void setPose(const Coordinates & pose_in) - { - pose = pose_in; - } + inline void setPose(const Coordinates & pose_in) { pose = pose_in; } + + // NOTE: If the start node is the same as the goal node we need to save the start pose + // separatly. Then the start pose will be in goal->pose_start and the goal will be in goal->pose. + inline void setPoseStart(const Coordinates & pose_in) { pose_start = pose_in; } /** * @brief Reset method for new search @@ -218,79 +205,57 @@ class NodeHybrid * @brief Gets the accumulated cost at this node * @return accumulated cost */ - inline float & getAccumulatedCost() - { - return _accumulated_cost; - } + inline float & getAccumulatedCost() { return _accumulated_cost; } /** * @brief Sets the accumulated cost at this node * @param reference to accumulated cost */ - inline void setAccumulatedCost(const float & cost_in) - { - _accumulated_cost = cost_in; - } + inline void setAccumulatedCost(const float & cost_in) { _accumulated_cost = cost_in; } /** * @brief Sets the motion primitive index used to achieve node in search * @param reference to motion primitive idx */ - inline void setMotionPrimitiveIndex(const unsigned int & idx) - { - _motion_primitive_index = idx; - } + inline void setMotionPrimitiveIndex(const unsigned int & idx) { _motion_primitive_index = idx; } /** * @brief Gets the motion primitive index used to achieve node in search * @return reference to motion primitive idx */ - inline unsigned int & getMotionPrimitiveIndex() - { - return _motion_primitive_index; - } + inline unsigned int & getMotionPrimitiveIndex() { return _motion_primitive_index; } /** * @brief Gets the costmap cost at this node * @return costmap cost */ - inline float & getCost() - { - return _cell_cost; - } + inline float & getCost() { return _cell_cost; } /** * @brief Gets if cell has been visited in search * @param If cell was visited */ - inline bool & wasVisited() - { - return _was_visited; - } + inline bool & wasVisited() { return _was_visited; } /** * @brief Sets if cell has been visited in search */ - inline void visited() - { - _was_visited = true; - } + inline void visited() { _was_visited = true; } /** * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int & getIndex() - { - return _index; - } + inline unsigned int & getIndex() { return _index; } /** * @brief Check if this node is valid * @param traverse_unknown If we can explore unknown nodes on the graph * @return whether this node is valid and collision free */ - bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker); + bool isNodeValid( + const bool & traverse_unknown, GridCollisionChecker * collision_checker, + bool traverse_cost = true); /** * @brief Get traversal cost of parent node to child node @@ -325,9 +290,7 @@ class NodeHybrid static inline unsigned int getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle) { - return getIndex( - x, y, angle, motion_table.size_x, - motion_table.num_angle_quantization); + return getIndex(x, y, angle, motion_table.size_x, motion_table.num_angle_quantization); } /** @@ -338,13 +301,12 @@ class NodeHybrid * @return Coordinates */ static inline Coordinates getCoords( - const unsigned int & index, - const unsigned int & width, const unsigned int & angle_quantization) + const unsigned int & index, const unsigned int & width, const unsigned int & angle_quantization) { return Coordinates( - (index / angle_quantization) % width, // x - index / (angle_quantization * width), // y - index % angle_quantization); // theta + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta } /** @@ -355,8 +317,7 @@ class NodeHybrid * @return Heuristic cost between the nodes */ static float getHeuristicCost( - const Coordinates & node_coords, - const Coordinates & goal_coordinates, + const Coordinates & node_coords, const Coordinates & goal_coordinates, const nav2_costmap_2d::Costmap2D * costmap); /** @@ -368,11 +329,8 @@ class NodeHybrid * @param search_info Search info to use */ static void initMotionModel( - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & angle_quantization, - SearchInfo & search_info); + const MotionModel & motion_model, unsigned int & size_x, unsigned int & size_y, + unsigned int & angle_quantization, SearchInfo & search_info); /** * @brief Compute the SE2 distance heuristic @@ -383,10 +341,8 @@ class NodeHybrid * @param search_info Info containing minimum radius to use */ static void precomputeDistanceHeuristic( - const float & lookup_table_dim, - const MotionModel & motion_model, - const unsigned int & dim_3_size, - const SearchInfo & search_info); + const float & lookup_table_dim, const MotionModel & motion_model, + const unsigned int & dim_3_size, const SearchInfo & search_info); /** * @brief Compute the Obstacle heuristic @@ -395,8 +351,7 @@ class NodeHybrid * @return heuristic Heuristic value */ static float getObstacleHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords); + const Coordinates & node_coords, const Coordinates & goal_coords, const double & cost_penalty); /** * @brief Compute the Distance heuristic @@ -407,8 +362,7 @@ class NodeHybrid * @return heuristic Heuristic value */ static float getDistanceHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords, + const Coordinates & node_coords, const Coordinates & goal_coords, const float & obstacle_heuristic); /** @@ -417,8 +371,8 @@ class NodeHybrid * @param goal_coords Coordinates to start heuristic expansion at */ static void resetObstacleHeuristic( - nav2_costmap_2d::Costmap2D * costmap, - const unsigned int & goal_x, const unsigned int & goal_y); + nav2_costmap_2d::Costmap2D * costmap, const unsigned int & start_x, + const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y); /** * @brief Retrieve all valid neighbors of a node. @@ -428,26 +382,40 @@ class NodeHybrid * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown, + std::function & validity_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); + /** + * @brief Set the starting pose for planning, as a node index + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(CoordinateVector & path); + NodeHybrid * parent; Coordinates pose; + Coordinates pose_start; // Constants required across all nodes but don't want to allocate more than once static double travel_distance_cost; static HybridMotionTable motion_table; // Wavefront lookup and queue for continuing to expand as needed static LookupTable obstacle_heuristic_lookup_table; - static std::queue obstacle_heuristic_queue; + static ObstacleHeuristicQueue obstacle_heuristic_queue; + static nav2_costmap_2d::Costmap2D * sampled_costmap; static CostmapDownsampler downsampler; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; + bool is_goal = false; + + // NOTE: If the goal and start is the same node, we need to keep these values separate. + float continuous_angle_goal; + float continuous_angle_start; + private: float _cell_cost; float _accumulated_cost; 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 4fd56731aca..e0c3093ec9f 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 @@ -16,26 +16,25 @@ #define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ #include -#include #include +#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/costmap_downsampler.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" -#include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" #include "tf2/utils.h" namespace nav2_smac_planner { - class SmacPlannerHybrid : public nav2_core::GlobalPlanner { public: @@ -57,8 +56,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner * @param costmap_ros Costmap2DROS object */ void configure( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, std::shared_ptr tf, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, + std::shared_ptr tf, std::shared_ptr costmap_ros) override; /** @@ -88,10 +87,11 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner protected: /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters */ - void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); std::unique_ptr> _a_star; GridCollisionChecker _collision_checker; @@ -113,15 +113,15 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; + double _minimum_turning_radius_global_coords; std::string _motion_model_for_search; MotionModel _motion_model; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; - // Subscription for parameter change - rclcpp::AsyncParametersClient::SharedPtr _parameters_client; - rclcpp::Subscription::SharedPtr _parameter_event_sub; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index a9c45d4cda2..d03fd48e89a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -16,18 +16,69 @@ #define NAV2_SMAC_PLANNER__SMOOTHER_HPP_ #include -#include #include #include #include #include +#include +#include "angles/angles.h" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav_msgs/msg/path.hpp" +#include "ompl/base/StateSpace.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "tf2/utils.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 @@ -39,13 +90,7 @@ class Smoother /** * @brief A constructor for nav2_smac_planner::Smoother */ - explicit Smoother(const SmootherParams & params) - { - tolerance_ = params.tolerance_; - max_its_ = params.max_its_; - data_w_ = params.w_data_; - smooth_w_ = params.w_smooth_; - } + explicit Smoother(const SmootherParams & params); /** * @brief A destructor for nav2_smac_planner::Smoother @@ -55,198 +100,129 @@ class 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) - { - min_turning_rad_ = min_turning_radius; - } + void initialize(const double & min_turning_radius); /** - * @brief Smoother method + * @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, - const bool do_refinement = true) - { - using namespace std::chrono; // NOLINT - 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_im2, y_ip2, y_i_org, curvature; - unsigned int mx, my; - - // Adding 5% margin due to floating point error - const double max_curvature = (1.0 / min_turning_rad_) * 1.05; - 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); - 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); - return false; - } - - for (unsigned int i = 2; i != path_size - 2; 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; - - if (i > 2 && i < path_size - 2) { - // Smooth based on local 5 point neighborhood and original data locations - y_im2 = getFieldByDim(new_path.poses[i - 2], j); - y_ip2 = getFieldByDim(new_path.poses[i + 2], j); - y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_im2 + y_ip2 + y_ip1 + y_m1 - (4.0 * y_i)); - } else { - // Smooth based on local 3 point neighborhood and original data locations - // At boundary conditions, need to use a more local neighborhood because the first - // and last 2 points cannot move to ensure the boundry conditions are upheld - 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) { - 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); - return false; - } - } - - last_path = new_path; - } - - // Lets do additional refinement, it shouldn't take more than a couple milliseconds - // but really puts the path quality over the top. - if (do_refinement) { - smooth(new_path, costmap, max_time, false); - } - - for (unsigned int i = 3; i != path_size - 3; i++) { - if (getCurvature(new_path, i) > max_curvature) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing process resulted in an infeasible curvature somewhere on the path. " - "This is most likely at the end point boundary conditions which will be further " - "refined as a perfect curve as you approach the goal. If this becomes a practical " - "issue for you, please file a ticket mentioning this message."); - updateApproximatePathOrientations(new_path); - path = new_path; - return false; - } - } - - updateApproximatePathOrientations(new_path); - path = new_path; - return true; - } + nav_msgs::msg::Path & path, const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); protected: - inline double 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; - } - } + /** + * @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 dimention to for the pose + */ inline void 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; - } - } + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - inline double getCurvature(const nav_msgs::msg::Path & path, const unsigned int i) - { - // k = 1 / r = acos(delta_phi) / |xi|, where delta_phi = (xi dot xi+1) / (|xi| * |xi+1|) - const double dxi_x = getFieldByDim(path.poses[i], 0) - getFieldByDim(path.poses[i - 1], 0); - const double dxi_y = getFieldByDim(path.poses[i], 1) - getFieldByDim(path.poses[i - 1], 1); - const double dxip1_x = getFieldByDim(path.poses[i + 1], 0) - getFieldByDim(path.poses[i], 0); - const double dxip1_y = getFieldByDim(path.poses[i + 1], 1) - getFieldByDim(path.poses[i], 1); - const double norm_dx_i = hypot(dxi_x, dxi_y); - const double norm_dx_ip1 = hypot(dxip1_x, dxip1_y); - double arg = (dxi_x * dxip1_x + dxi_y * dxip1_y) / (norm_dx_i * norm_dx_ip1); - - // In case of small out of bounds issues from floating point error - if (arg > 1.0) { - arg = 1.0; - } else if (arg < -1.0) { - arg = -1.0; - } - - return acos(arg) / norm_dx_i; - } + /** + * @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); - inline void updateApproximatePathOrientations(nav_msgs::msg::Path & path) - { - using namespace nav2_util::geometry_utils; // NOLINT - double dx, dy, theta; - 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); - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } - } + /** + * @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_; + int max_its_, refinement_ctr_; + bool is_holonomic_, do_refinement_; + MotionModel motion_model_; + ompl::base::StateSpacePtr state_space_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index dffc88062bc..b425227d6e0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -15,17 +15,16 @@ #ifndef NAV2_SMAC_PLANNER__TYPES_HPP_ #define NAV2_SMAC_PLANNER__TYPES_HPP_ -#include -#include -#include #include +#include +#include +#include -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace nav2_smac_planner { - typedef std::pair NodeHeuristicPair; /** @@ -39,9 +38,13 @@ struct SearchInfo float change_penalty; float reverse_penalty; float cost_penalty; + float retrospective_penalty; + float rotation_penalty; float analytic_expansion_ratio; + float analytic_expansion_max_length; std::string lattice_filepath; bool cache_obstacle_heuristic; + bool allow_reverse_expansion; }; /** @@ -53,9 +56,7 @@ struct SmootherParams /** * @brief A constructor for nav2_smac_planner::SmootherParams */ - SmootherParams() - { - } + SmootherParams() : holonomic_(false) {} /** * @brief Get params from ROS parameter @@ -79,14 +80,84 @@ struct SmootherParams nav2_util::declare_parameter_if_not_declared( node, local_name + "w_smooth", rclcpp::ParameterValue(0.3)); node->get_parameter(local_name + "w_smooth", w_smooth_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "do_refinement", rclcpp::ParameterValue(true)); + node->get_parameter(local_name + "do_refinement", do_refinement_); } double tolerance_; int max_its_; double w_data_; double w_smooth_; + bool holonomic_; + bool do_refinement_; +}; + +/** + * @struct nav2_smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for nav2_smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for nav2_smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + */ + MotionPose(const float & x, const float & y, const float & theta) : _x(x), _y(y), _theta(theta) {} + + MotionPose operator-(const MotionPose & p2) + { + return MotionPose(this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta); + } + + float _x; + float _y; + float _theta; }; +typedef std::vector MotionPoses; + +/** + * @struct nav2_smac_planner::LatticeMetadata + * @brief A struct of all lattice metadata + */ +struct LatticeMetadata +{ + float min_turning_radius; + float grid_resolution; + unsigned int number_of_headings; + std::vector heading_angles; + unsigned int number_of_trajectories; + std::string motion_model; +}; + +/** + * @struct nav2_smac_planner::MotionPrimitive + * @brief A struct of all motion primitive data + */ +struct MotionPrimitive +{ + unsigned int trajectory_id; + float start_angle; + float end_angle; + float turning_radius; + float trajectory_length; + float arc_length; + float straight_length; + bool left_turn; + MotionPoses poses; +}; + +typedef std::vector MotionPrimitives; +typedef std::vector MotionPrimitivePtrs; + } // namespace nav2_smac_planner #endif // NAV2_SMAC_PLANNER__TYPES_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index 240152dd21e..ae85f30f405 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -15,19 +15,19 @@ #ifndef NAV2_SMAC_PLANNER__UTILS_HPP_ #define NAV2_SMAC_PLANNER__UTILS_HPP_ -#include #include +#include +#include #include "Eigen/Core" -#include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "tf2/utils.h" +#include "geometry_msgs/msg/quaternion.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" +#include "relox_costmap_2d/inflation_layer.hpp" +#include "tf2/utils.h" namespace nav2_smac_planner { - /** * @brief Create an Eigen Vector2D of world poses from continuous map coords * @param mx float of map X coordinate @@ -58,17 +58,15 @@ inline geometry_msgs::msg::Pose getMapCoords( } /** -* @brief Create quaternion from A* coord bins +* @brief Create quaternion from radians * @param theta continuous bin coordinates angle * @return quaternion orientation in map frame */ -inline geometry_msgs::msg::Quaternion getWorldOrientation( - const float & theta, - const float & bin_size) +inline geometry_msgs::msg::Quaternion getWorldOrientation(const float & theta) { - // theta is in continuous bin coordinates, must convert to world orientation + // theta is in radians already tf2::Quaternion q; - q.setEuler(0.0, 0.0, theta * static_cast(bin_size)); + q.setEuler(0.0, 0.0, theta); return tf2::toMsg(q); } @@ -87,11 +85,9 @@ inline double findCircumscribedCost(std::shared_ptrgetLayeredCostmap()->getPlugins()->begin(); - layer != costmap->getLayeredCostmap()->getPlugins()->end(); - ++layer) - { - std::shared_ptr inflation_layer = - std::dynamic_pointer_cast(*layer); + layer != costmap->getLayeredCostmap()->getPlugins()->end(); ++layer) { + std::shared_ptr inflation_layer = + std::dynamic_pointer_cast(*layer); if (!inflation_layer) { continue; } @@ -115,6 +111,57 @@ inline double findCircumscribedCost(std::shared_ptr nav2_smac_planner - 1.0.12 + 1.0.0 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 @@ -26,6 +26,9 @@ eigen3_cmake_module eigen ompl + nlohmann-json-dev + angles + relox_costmap_2d ament_lint_common ament_lint_auto @@ -34,6 +37,8 @@ ament_cmake - + + + diff --git a/nav2_smac_planner/smac_plugin_hybrid.xml b/nav2_smac_planner/smac_plugin_hybrid.xml new file mode 100644 index 00000000000..7b52bb24d76 --- /dev/null +++ b/nav2_smac_planner/smac_plugin_hybrid.xml @@ -0,0 +1,5 @@ + + + Hybrid-A* SMAC planner + + diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 8e460ce6d87..2fe61995ff6 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -13,35 +13,32 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include +#include "nav2_smac_planner/a_star.hpp" -#include -#include -#include +#include -#include -#include -#include #include -#include -#include #include +#include +#include +#include +#include #include +#include #include #include -#include "nav2_smac_planner/a_star.hpp" +#include "geometry_msgs/msg/pose_array.hpp" using namespace std::chrono; // NOLINT namespace nav2_smac_planner { - -template +template AStarAlgorithm::AStarAlgorithm( - const MotionModel & motion_model, - const SearchInfo & search_info) + const MotionModel & motion_model, const SearchInfo & search_info) : _traverse_unknown(true), _max_iterations(0), + _max_planning_time(0), _x_size(0), _y_size(0), _search_info(search_info), @@ -53,45 +50,28 @@ AStarAlgorithm::AStarAlgorithm( _graph.reserve(100000); } -template +template AStarAlgorithm::~AStarAlgorithm() { } -template +template void AStarAlgorithm::initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const float & lookup_table_size, + const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const double & max_planning_time, const float & lookup_table_size, const unsigned int & dim_3_size) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _max_planning_time = max_planning_time; NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); _dim3_size = dim_3_size; + _expander = std::make_unique>( + _motion_model, _search_info, _traverse_unknown, _dim3_size); } -template<> -void AStarAlgorithm::initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const float & /*lookup_table_size*/, - const unsigned int & dim_3_size) -{ - _traverse_unknown = allow_unknown; - _max_iterations = max_iterations; - _max_on_approach_iterations = max_on_approach_iterations; - - if (dim_3_size != 1) { - throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); - } - _dim3_size = dim_3_size; -} - -template +template void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) { _collision_checker = collision_checker; @@ -106,9 +86,10 @@ void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision _y_size = y_size; NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); } + _expander->setCollisionChecker(collision_checker); } -template +template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { @@ -117,76 +98,59 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( return &(_graph.emplace(index, NodeT(index)).first->second); } -template<> -void AStarAlgorithm::setStart( - const unsigned int & mx, - const unsigned int & my, - const unsigned int & dim_3, - const double & cmx, - const double & cmy) -{ - if (dim_3 != 0) { - throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); - } - _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); -} - -template +template void AStarAlgorithm::setStart( - const unsigned int & mx, - const unsigned int & my, - const unsigned int & dim_3, - const double & cmx, - const double & cmy) + const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3, const double & cmx, + const double & cmy, const double & cdim_3) { _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); _start->setPose( - Coordinates( - static_cast(cmx), - static_cast(cmy), - static_cast(dim_3))); -} + Coordinates(static_cast(cmx), static_cast(cmy), static_cast(dim_3))); + _start->setPoseStart( + Coordinates(static_cast(cmx), static_cast(cmy), static_cast(dim_3))); + _start->setMotionPrimitiveIndex(0); + _start->continuous_angle_start = cdim_3; -template<> -void AStarAlgorithm::setGoal( - const unsigned int & mx, - const unsigned int & my, - const unsigned int & dim_3, - const double & cmx, - const double & cmy) -{ - if (dim_3 != 0) { - throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); - } - - _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + // std::cout << "\n\nsetStart: _start->pose_start.x: " << _start->pose_start.x + // << ", _start->pose_start.y: " << _start->pose_start.y << std::endl; + // std::cout << "\n\nsetStart: cmx: " << cmx << ", cmy: " << cmy << std::endl; } -template +template void AStarAlgorithm::setGoal( - const unsigned int & mx, - const unsigned int & my, - const unsigned int & dim_3, - const double & cmx, - const double & cmy) + const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3, const double & cmx, + const double & cmy, const double & cdim_3) { + // std::cout << "\n\nsetGoal (begin): _start->pose_start.x: " << _start->pose_start.x + // << ", _start->pose_start.y: " << _start->pose_start.y << std::endl; + _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); typename NodeT::Coordinates goal_coords( - static_cast(cmx), - static_cast(cmy), - static_cast(dim_3)); + static_cast(cmx), static_cast(cmy), static_cast(dim_3)); if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { - NodeT::resetObstacleHeuristic(_costmap, mx, my); + if (!_start) { + throw std::runtime_error("Start must be set before goal."); + } + + NodeT::resetObstacleHeuristic(_costmap, _start->pose_start.x, _start->pose_start.y, mx, my); } _goal_coordinates = goal_coords; _goal->setPose(_goal_coordinates); + _goal->is_goal = true; + _goal->continuous_angle_goal = cdim_3; + + // std::cout << "\n\nsetGoal: _goal->pose.x: " << _goal->pose.x + // << ", _goal->pose.y: " << _goal->pose.y << std::endl; + // std::cout << "\n\nsetGoal: cmx: " << cmx << ", cmy: " << cmy << std::endl; + + // std::cout << "\n\nsetGoal (end): _start->pose_start.x: " << _start->pose_start.x + // << ", _start->pose_start.y: " << _start->pose_start.y << std::endl; } -template +template bool AStarAlgorithm::areInputsValid() { // Check if graph was filled in @@ -200,9 +164,9 @@ bool AStarAlgorithm::areInputsValid() } // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { + if ( + /*getToleranceHeuristic() < 0.001 && */ !_goal->isNodeValid( + _traverse_unknown, _collision_checker)) { throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); } @@ -214,16 +178,28 @@ bool AStarAlgorithm::areInputsValid() return true; } -template +template bool AStarAlgorithm::createPath( - CoordinateVector & path, int & iterations, - const float & tolerance) -{ + CoordinateVector & path, int & iterations, const float & tolerance) +{ +#ifdef PUB_EXPANSION + static auto node = std::make_shared("test"); + static auto pub = node->create_publisher("expansions", 1); + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = node->now(); + msg.header.frame_id = "map"; +#endif + + steady_clock::time_point start_time = steady_clock::now(); _tolerance = tolerance; _best_heuristic_node = {std::numeric_limits::max(), 0}; clearQueue(); if (!areInputsValid()) { +#ifdef PUB_EXPANSION + pub->publish(msg); +#endif return false; } @@ -244,18 +220,113 @@ bool AStarAlgorithm::createPath( // Given an index, return a node ptr reference if its collision-free and valid const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); - NodeGetter neighborGetter = - [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool - { - if (index < 0 || index >= max_index) { - return false; + NodeGetter neighborGetter = [&, this]( + const unsigned int & index, NodePtr & neighbor_rtn) -> bool { + if (index < 0 || index >= max_index) { + return false; + } + + // if (this->_goal->getIndex() == index) { + // std::cout << "Goal is neighbor" << std::endl; + // } + + neighbor_rtn = addToGraph(index); + return true; + }; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // Special case checker that looks if the start and goal pose are exactly in front of eachother. + // This happens a lot when doing lane coverage at Relox. + + auto start = getStart(); + // https://stackoverflow.com/questions/1878907/how-can-i-find-the-difference-between-two-angles + float angle_diff = start->continuous_angle_start - _goal->continuous_angle_goal; + angle_diff = fabs(atan2(sin(angle_diff), cos(angle_diff))); + // std::cout << "\n\nANGLE DIFF IS: " << angle_diff << "\n\n" << std::endl; + if (angle_diff < 15.0 * M_PI / 180.0) { + float dx = _goal->pose.x - start->pose_start.x; + float dy = _goal->pose.y - start->pose_start.y; + // std::cout << "\n\nSTART POSE X: " << _start->pose_start.x + // << ", START POSE Y: " << _start->pose_start.y << "\n\n" + // << std::endl; + // std::cout << "\n\nGOAL POSE X: " << _goal->pose.x << ", GOAL POSE Y: " << _goal->pose.y + // << "\n\n" + // << std::endl; + float l = sqrt(dx * dx + dy * dy); + // dir of theta rotated 90 degrees counter clockwise (left) + float left_dir_x = -sin(start->continuous_angle_start); + float left_dir_y = cos(start->continuous_angle_start); + float left_dot = (dx / l) * left_dir_x + (dy / l) * left_dir_y; + // dir of theta (forward) + float forward_dir_x = cos(start->continuous_angle_start); + float forward_dir_y = sin(start->continuous_angle_start); + float forward_dot = (dx / l) * forward_dir_x + (dy / l) * forward_dir_y; + // std::cout << "\n\n*********\n\nGOAL AND START THETA ARE THE SAME!!! fabs(left_dot)-> (" + // << fabs(left_dot) << ") forward_dot -> (" << forward_dot << ") angle_diff -> ( " + // << angle_diff << " ) l -> ( " << l << " )\n\n*********\n\n" + // << std::endl; + + if (fabs(left_dot) < 0.2 && forward_dot >= 0.0) { + // std::cout << "\n\n********\n\nAND THEY ARE ALSO COLLINEAR!!!\n\n********\n\n" << std::endl; + + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(l / sqrt_2); + + bool ok = true; + // its ok to traverse over cost if we are closer than 3 meters + const float max_l = 3.0 / _costmap->getResolution(); + + // Check intermediary poses (non-goal, non-start) + for (float i = 1; i < num_intervals; i++) { + float ix = start->pose_start.x + dx * (i / num_intervals); + float iy = start->pose_start.y + dy * (i / num_intervals); + // std::cout << "start x: " << start->pose_start.x << " ix: " << ix << " goal x" + // << _goal->pose.x << std::endl; + if (_collision_checker->inCollision(ix, iy, start->pose_start.theta, false)) { + // not ok! + // std::cout << "IN COLLISION -> NOT OK!!!" << std::endl; + ok = false; + break; + } else { + // std::cout << "NOT IN COLLISION!!!" << std::endl; + if (_collision_checker->getCost() == 0.0 || l < max_l) { + // ok! + // std::cout << "COST IS ZERO OR l < max_l -> OK!!! l=" << l << std::endl; + } else { + // not ok! + // std::cout << "COST IS HIGH!!! -> NOT OK l=" << l << std::endl; + ok = false; + break; + } + } } - neighbor_rtn = addToGraph(index); - return true; - }; + if (ok) { + _goal->parent = start; +#ifdef PUB_EXPANSION + pub->publish(msg); +#endif + return _goal->backtracePath(path); + } + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// while (iterations < getMaxIterations() && !_queue.empty()) { + // Check for planning timeout only on every Nth iteration + if (iterations % _timing_interval == 0) { + std::chrono::duration planning_duration = + std::chrono::duration_cast>(steady_clock::now() - start_time); + if (static_cast(planning_duration.count()) >= _max_planning_time) { +#ifdef PUB_EXPANSION + pub->publish(msg); +#endif + return false; + } + } + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue current_node = getNextNode(); @@ -265,6 +336,17 @@ bool AStarAlgorithm::createPath( continue; } +#ifdef PUB_EXPANSION + msg_pose.position.x = + _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution()); + msg_pose.position.y = + _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution()); + tf2::Quaternion q; + q.setEuler(0.0, 0.0, NodeHybrid::motion_table.getAngleFromBin(current_node->pose.theta)); + msg_pose.orientation = tf2::toMsg(q); + msg.poses.push_back(msg_pose); +#endif + iterations++; // 2) Mark Nbest as visited @@ -272,20 +354,36 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; - expansion_result = tryAnalyticExpansion( - current_node, neighborGetter, analytic_iterations, closest_distance); + expansion_result = _expander->tryAnalyticExpansion( + current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { + // std::cout << "\n\nUSED ANALYTICAL EXPANSION!!!!!!!!!!!!!\n\n" << std::endl; current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required + // std::cout << _best_heuristic_node.first << " < " << getToleranceHeuristic() << " | " + // << approach_iterations << " >= " << getOnApproachMaxIterations() << std::endl; if (isGoal(current_node)) { - return backtracePath(current_node, path); + _goal->parent = current_node->parent; + _goal->visited(); +// std::cout << "_goal -> x: " << _goal->pose.x << " y: " << _goal->pose.y +// << " theta: " << _goal->pose.theta << std::endl; +#ifdef PUB_EXPANSION + pub->publish(msg); +#endif + return _goal->backtracePath(path); } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason approach_iterations++; if (approach_iterations >= getOnApproachMaxIterations()) { - return backtracePath(&_graph.at(_best_heuristic_node.second), path); + // std::cout << "\n\nCOULD ONLY REACHED GOAL WITHIN TOLERANCE\n\n" << std::endl; + _goal->parent = _graph.at(_best_heuristic_node.second).parent; + _goal->visited(); +#ifdef PUB_EXPANSION + pub->publish(msg); +#endif + return _goal->backtracePath(path); } } @@ -293,9 +391,8 @@ bool AStarAlgorithm::createPath( neighbors.clear(); current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); - for (neighbor_iterator = neighbors.begin(); - neighbor_iterator != neighbors.end(); ++neighbor_iterator) - { + for (neighbor_iterator = neighbors.begin(); neighbor_iterator != neighbors.end(); + ++neighbor_iterator) { neighbor = *neighbor_iterator; // 4.1) Compute the cost to go to this node @@ -312,117 +409,52 @@ bool AStarAlgorithm::createPath( } } +#ifdef PUB_EXPANSION + pub->publish(msg); +#endif return false; } -template +template bool AStarAlgorithm::isGoal(NodePtr & node) { return node == getGoal(); } -template<> -bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) -{ - if (!node->parent) { - return false; - } - - NodePtr current_node = node; - - while (current_node->parent) { - path.push_back( - Node2D::getCoords( - current_node->getIndex(), getSizeX(), getSizeDim3())); - current_node = current_node->parent; - } - - path.push_back( - Node2D::getCoords( - current_node->getIndex(), getSizeX(), getSizeDim3())); - - return path.size() > 0; -} - -template -bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) -{ - if (!node->parent) { - return false; - } - - NodePtr current_node = node; - - while (current_node->parent) { - path.push_back(current_node->pose); - current_node = current_node->parent; - } - - path.push_back(current_node->pose); - - return path.size() > 0; -} - -template +template typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() { return _start; } -template +template typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() { return _goal; } -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() -{ - NodeBasic node = _queue.top().second; - _queue.pop(); - return node.graph_node_ptr; -} - -template +template typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { NodeBasic node = _queue.top().second; _queue.pop(); - - // We only want to override the node's pose if it has not yet been visited - // to prevent the case that a node has been queued multiple times and - // a new branch is overriding one of lower cost already visited. - if (!node.graph_node_ptr->wasVisited()) { - node.graph_node_ptr->pose = node.pose; - } - + node.processSearchNode(); return node.graph_node_ptr; } -template<> -void AStarAlgorithm::addNode(const float & cost, NodePtr & node) -{ - NodeBasic queued_node(node->getIndex()); - queued_node.graph_node_ptr = node; - _queue.emplace(cost, queued_node); -} - -template +template void AStarAlgorithm::addNode(const float & cost, NodePtr & node) { NodeBasic queued_node(node->getIndex()); - queued_node.pose = node->pose; - queued_node.graph_node_ptr = node; + queued_node.populateSearchNode(node); _queue.emplace(cost, queued_node); } -template +template float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { - const Coordinates node_coords = - NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates, _costmap); + const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); + float heuristic = NodeT::getHeuristicCost(node_coords, _goal_coordinates, _costmap); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; @@ -431,14 +463,14 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) return heuristic; } -template +template void AStarAlgorithm::clearQueue() { NodeQueue q; std::swap(_queue, q); } -template +template void AStarAlgorithm::clearGraph() { Graph g; @@ -446,240 +478,43 @@ void AStarAlgorithm::clearGraph() _graph.reserve(100000); } -template +template int & AStarAlgorithm::getMaxIterations() { return _max_iterations; } -template +template int & AStarAlgorithm::getOnApproachMaxIterations() { return _max_on_approach_iterations; } -template +template float & AStarAlgorithm::getToleranceHeuristic() { return _tolerance; } -template +template unsigned int & AStarAlgorithm::getSizeX() { return _x_size; } -template +template unsigned int & AStarAlgorithm::getSizeY() { return _y_size; } -template +template unsigned int & AStarAlgorithm::getSizeDim3() { return _dim3_size; } -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpansion( - const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations, - int & closest_distance) -{ - // This must be a NodeHybrid or NodeLattice if we are using these motion models - if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || - _motion_model == MotionModel::STATE_LATTICE) - { - // See if we are closer and should be expanding more often - const Coordinates node_coords = - NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3()); - closest_distance = std::min( - closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, _goal_coordinates, _costmap))); - - // We want to expand at a rate of d/expansion_ratio, - // but check to see if we are so close that we would be expanding every iteration - // If so, limit it to the expansion ratio (rounded up) - int desired_iterations = std::max( - static_cast(closest_distance / _search_info.analytic_expansion_ratio), - static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - - // If we are closer now, we should update the target number of iterations to go - analytic_iterations = - std::min(analytic_iterations, desired_iterations); - - // Always run the expansion on the first run in case there is a - // trivial path to be found - if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; - AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, getter); - if (!analytic_nodes.empty()) { - // If we have a valid path, attempt to refine it - NodePtr node = current_node; - NodePtr test_node = current_node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - refined_analytic_nodes = getAnalyticPath(test_node, getter); - if (refined_analytic_nodes.empty()) { - break; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; - } - } - - return setAnalyticPath(node, analytic_nodes); - } - } - - analytic_iterations--; - } - - // No valid motion model - return nullptr - return NodePtr(nullptr); -} - -template -typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::getAnalyticPath( - const NodePtr & node, - const NodeGetter & node_getter) -{ - static ompl::base::ScopedState<> from(node->motion_table.state_space), to( - node->motion_table.state_space), s(node->motion_table.state_space); - from[0] = node->pose.x; - from[1] = node->pose.y; - from[2] = node->pose.theta * node->motion_table.bin_size; - to[0] = _goal_coordinates.x; - to[1] = _goal_coordinates.y; - to[2] = _goal_coordinates.theta * node->motion_table.bin_size; - - float d = node->motion_table.state_space->distance(from(), to()); - - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = std::sqrt(2.); - unsigned int num_intervals = std::floor(d / sqrt_2); - - AnalyticExpansionNodes possible_nodes; - // When "from" and "to" are zero or one cell away, - // num_intervals == 0 - possible_nodes.reserve(num_intervals); // We won't store this node or the goal - std::vector reals; - - // Pre-allocate - NodePtr prev(node); - unsigned int index = 0; - NodePtr next(nullptr); - float angle = 0.0; - Coordinates proposed_coordinates; - bool failure = false; - - // Check intermediary poses (non-goal, non-start) - for (float i = 1; i < num_intervals; i++) { - node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); - reals = s.reals(); - angle = reals[2] / node->motion_table.bin_size; - while (angle < 0.0) { - angle += node->motion_table.num_angle_quantization_float; - } - while (angle >= node->motion_table.num_angle_quantization_float) { - angle -= node->motion_table.num_angle_quantization_float; - } - // Turn the pose into a node, and check if it is valid - index = NodeT::getIndex( - static_cast(reals[0]), - static_cast(reals[1]), - static_cast(angle)); - // Get the node from the graph - if (node_getter(index, next)) { - Coordinates initial_node_coords = next->pose; - proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; - next->setPose(proposed_coordinates); - if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { - // Save the node, and its previous coordinates in case we need to abort - possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); - prev = next; - } else { - // Abort - next->setPose(initial_node_coords); - failure = true; - break; - } - } else { - // Abort - failure = true; - break; - } - } - - // Reset to initial poses to not impact future searches - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.node; - n->setPose(node_pose.initial_coords); - } - - if (failure) { - return AnalyticExpansionNodes(); - } - - return possible_nodes; -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::setAnalyticPath( - const NodePtr & node, - const AnalyticExpansionNodes & expanded_nodes) -{ - // Legitimate final path - set the parent relationships & poses - NodePtr prev = node; - for (const auto & node_pose : expanded_nodes) { - const auto & n = node_pose.node; - if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { - // Make sure this node has not been visited by the regular algorithm. - // If it has been, there is the (slight) chance that it is in the path we are expanding - // from, so we should skip it. - // Skipping to the next node will still create a kinematically feasible path. - n->parent = prev; - n->pose = node_pose.proposed_coords; - n->visited(); - prev = n; - } - } - if (_goal != prev) { - _goal->parent = prev; - _goal->visited(); - } - return _goal; -} - -template<> -typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::getAnalyticPath( - const NodePtr & node, - const NodeGetter & node_getter) -{ - return AnalyticExpansionNodes(); -} - -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::setAnalyticPath( - const NodePtr & node, - const AnalyticExpansionNodes & expanded_nodes) -{ - return NodePtr(nullptr); -} - // Instantiate algorithm for the supported template types -template class AStarAlgorithm; template class AStarAlgorithm; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp new file mode 100644 index 00000000000..dd7d00d1d74 --- /dev/null +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -0,0 +1,243 @@ +// 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 "nav2_smac_planner/analytic_expansion.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace nav2_smac_planner +{ +template +AnalyticExpansion::AnalyticExpansion( + const MotionModel & motion_model, const SearchInfo & search_info, const bool & traverse_unknown, + const unsigned int & dim_3_size) +: _motion_model(motion_model), + _search_info(search_info), + _traverse_unknown(traverse_unknown), + _dim_3_size(dim_3_size), + _collision_checker(nullptr) +{ +} + +template +void AnalyticExpansion::setCollisionChecker(GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( + const NodePtr & current_node, const NodePtr & goal_node, const NodeGetter & getter, + int & analytic_iterations, int & closest_distance) +{ + // This must be a valid motion model for analytic expansion to be attempted + if ( + _motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || + _motion_model == MotionModel::STATE_LATTICE) { + // See if we are closer and should be expanding more often + auto costmap = _collision_checker->getCostmap(); + const Coordinates node_coords = + NodeT::getCoords(current_node->getIndex(), costmap->getSizeInCellsX(), _dim_3_size); + closest_distance = std::min( + closest_distance, + static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose, costmap))); + + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, goal_node, getter); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if ( + test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) { + test_node = test_node->parent->parent->parent->parent->parent; + refined_analytic_nodes = getAnalyticPath(test_node, goal_node, getter); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + return setAnalyticPath(node, goal_node, analytic_nodes); + } + } + + analytic_iterations--; + } + + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +template +typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( + const NodePtr & node, const NodePtr & goal, const NodeGetter & node_getter) +{ + static ompl::base::ScopedState<> from(node->motion_table.state_space), + to(node->motion_table.state_space), s(node->motion_table.state_space); + from[0] = node->pose.x; + from[1] = node->pose.y; + from[2] = node->motion_table.getAngleFromBin(node->pose.theta); + to[0] = goal->pose.x; + to[1] = goal->pose.y; + to[2] = node->motion_table.getAngleFromBin(goal->pose.theta); + + float d = node->motion_table.state_space->distance(from(), to()); + + // If the length is too far, exit. This prevents unsafe shortcutting of paths + // into higher cost areas far out from the goal itself, let search to the work of getting + // close before the analytic expansion brings it home. This should never be smaller than + // 4-5x the minimum turning radius being used, or planning times will begin to spike. + if (d > _search_info.analytic_expansion_max_length) { + return AnalyticExpansionNodes(); + } + + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(d / sqrt_2); + + AnalyticExpansionNodes possible_nodes; + // When "from" and "to" are zero or one cell away, + // num_intervals == 0 + possible_nodes.reserve(num_intervals); // We won't store this node or the goal + std::vector reals; + double theta; + + // Pre-allocate + NodePtr prev(node); + unsigned int index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + bool failure = false; + + _collision_checker->inCollision( + goal->pose.x, goal->pose.y, goal->pose.theta /*bin number*/, _traverse_unknown); + bool traverse_cost = d < 5.0; // (_collision_checker->getCost() > 0.0 && d < 5.0) || d < 5.0); + + // Check intermediary poses (non-goal, non-start) + for (float i = 1; i < num_intervals; i++) { + node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, 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; + angle = node->motion_table.getClosestAngularBin(theta); + + // Turn the pose into a node, and check if it is valid + index = NodeT::getIndex( + static_cast(reals[0]), static_cast(reals[1]), + static_cast(angle)); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker, traverse_cost) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); + prev = next; + } else { + // Abort + next->setPose(initial_node_coords); + failure = true; + break; + } + } else { + // Abort + failure = true; + break; + } + } + + // Reset to initial poses to not impact future searches + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.node; + n->setPose(node_pose.initial_coords); + } + + if (failure) { + return AnalyticExpansionNodes(); + } + + return possible_nodes; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( + const NodePtr & node, const NodePtr & goal_node, const AnalyticExpansionNodes & expanded_nodes) +{ + _detached_nodes.clear(); + // Legitimate final path - set the parent relationships, states, and poses + NodePtr prev = node; + for (const auto & node_pose : expanded_nodes) { + auto n = node_pose.node; + cleanNode(n); + if (n->getIndex() != goal_node->getIndex()) { + if (n->wasVisited()) { + _detached_nodes.push_back(std::make_unique(-1)); + n = _detached_nodes.back().get(); + } + n->parent = prev; + n->pose = node_pose.proposed_coords; + n->visited(); + prev = n; + } + } + if (goal_node != prev) { + goal_node->parent = prev; + cleanNode(goal_node); + goal_node->visited(); + } + return goal_node; +} + +template +void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) +{ +} + +template class AnalyticExpansion; + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp new file mode 100644 index 00000000000..12fea2bdbfc --- /dev/null +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -0,0 +1,168 @@ +// 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 "nav2_smac_planner/collision_checker.hpp" + +namespace nav2_smac_planner +{ +GridCollisionChecker::GridCollisionChecker( + nav2_costmap_2d::Costmap2D * costmap, unsigned int num_quantizations) +: FootprintCollisionChecker(costmap) +{ + // Convert number of regular bins into angles + float bin_size = 2 * M_PI / static_cast(num_quantizations); + angles_.reserve(num_quantizations); + for (unsigned int i = 0; i != num_quantizations; i++) { + angles_.push_back(bin_size * i); + } +} + +// GridCollisionChecker::GridCollisionChecker( +// nav2_costmap_2d::Costmap2D * costmap, +// std::vector & angles) +// : FootprintCollisionChecker(costmap), +// angles_(angles) +// { +// } + +void GridCollisionChecker::setFootprint( + const nav2_costmap_2d::Footprint & footprint, const bool & radius, + const double & possible_inscribed_cost) +{ + possible_inscribed_cost_ = possible_inscribed_cost; + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + oriented_footprints_.reserve(angles_.size()); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != angles_.size(); i++) { + sin_th = sin(angles_[i]); + cos_th = cos(angles_[i]); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; +} + +bool GridCollisionChecker::inCollision( + const float & x, const float & y, const float & angle_bin, const bool & traverse_unknown) +{ + // Check to make sure cell is inside the map + if ( + outsideRange(costmap_->getSizeInCellsX(), x) || outsideRange(costmap_->getSizeInCellsY(), y)) { + return false; + } + + // Assumes setFootprint already set + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + footprint_cost_ = costmap_->getCost(static_cast(x), static_cast(y)); + + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + footprint_cost_ = footprintCost(current_footprint); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = costmap_->getCost(static_cast(x), static_cast(y)); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return static_cast(footprint_cost_) >= INSCRIBED; + } +} + +bool GridCollisionChecker::inCollision(const unsigned int & i, const bool & traverse_unknown) +{ + footprint_cost_ = costmap_->getCost(i); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; +} + +float GridCollisionChecker::getCost() +{ + // Assumes inCollision called prior + return static_cast(footprint_cost_); +} + +bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) +{ + return value < 0.0f || value > max; +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp index 725ce5f469c..3eeb800e789 100644 --- a/nav2_smac_planner/src/costmap_downsampler.cpp +++ b/nav2_smac_planner/src/costmap_downsampler.cpp @@ -15,38 +15,32 @@ #include "nav2_smac_planner/costmap_downsampler.hpp" -#include -#include #include +#include +#include namespace nav2_smac_planner { - CostmapDownsampler::CostmapDownsampler() -: _costmap(nullptr), - _downsampled_costmap(nullptr), - _downsampled_costmap_pub(nullptr) +: _costmap(nullptr), _downsampled_costmap(nullptr), _downsampled_costmap_pub(nullptr) { } -CostmapDownsampler::~CostmapDownsampler() -{ -} +CostmapDownsampler::~CostmapDownsampler() {} void CostmapDownsampler::on_configure( - const nav2_util::LifecycleNode::WeakPtr & node, - const std::string & global_frame, - const std::string & topic_name, - nav2_costmap_2d::Costmap2D * const costmap, - const unsigned int & downsampling_factor) + const nav2_util::LifecycleNode::WeakPtr & node, const std::string & global_frame, + const std::string & topic_name, nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, const bool & use_min_cost_neighbor) { _costmap = costmap; _downsampling_factor = downsampling_factor; + _use_min_cost_neighbor = use_min_cost_neighbor; updateCostmapSize(); _downsampled_costmap = std::make_unique( - _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, - _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, _costmap->getOriginX(), + _costmap->getOriginY(), UNKNOWN); if (!node.expired()) { _downsampled_costmap_pub = std::make_unique( @@ -82,10 +76,10 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( updateCostmapSize(); // Adjust costmap size if needed - if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + if ( + _downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || - _downsampled_costmap->getResolution() != _downsampled_resolution) - { + _downsampled_costmap->getResolution() != _downsampled_resolution) { resizeCostmap(); } @@ -114,19 +108,14 @@ void CostmapDownsampler::updateCostmapSize() void CostmapDownsampler::resizeCostmap() { _downsampled_costmap->resizeMap( - _downsampled_size_x, - _downsampled_size_y, - _downsampled_resolution, - _costmap->getOriginX(), + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, _costmap->getOriginX(), _costmap->getOriginY()); } -void CostmapDownsampler::setCostOfCell( - const unsigned int & new_mx, - const unsigned int & new_my) +void CostmapDownsampler::setCostOfCell(const unsigned int & new_mx, const unsigned int & new_my) { unsigned int mx, my; - unsigned char cost = 0; + unsigned char cost = _use_min_cost_neighbor ? 255 : 0; unsigned int x_offset = new_mx * _downsampling_factor; unsigned int y_offset = new_my * _downsampling_factor; @@ -140,7 +129,8 @@ void CostmapDownsampler::setCostOfCell( if (my >= _size_y) { continue; } - cost = std::max(cost, _costmap->getCost(mx, my)); + cost = _use_min_cost_neighbor ? std::min(cost, _costmap->getCost(mx, my)) + : std::max(cost, _costmap->getCost(mx, my)); } } diff --git a/nav2_smac_planner/src/node_basic.cpp b/nav2_smac_planner/src/node_basic.cpp new file mode 100644 index 00000000000..c63430098a4 --- /dev/null +++ b/nav2_smac_planner/src/node_basic.cpp @@ -0,0 +1,41 @@ +// 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 "nav2_smac_planner/node_basic.hpp" + +namespace nav2_smac_planner +{ +template <> +void NodeBasic::processSearchNode() +{ + // We only want to override the node's pose if it has not yet been visited + // to prevent the case that a node has been queued multiple times and + // a new branch is overriding one of lower cost already visited. + if (!this->graph_node_ptr->wasVisited()) { + this->graph_node_ptr->pose = this->pose; + this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index); + } +} + +template <> +void NodeBasic::populateSearchNode(NodeHybrid *& node) +{ + this->pose = node->pose; + this->graph_node_ptr = node; + this->motion_index = node->getMotionPrimitiveIndex(); +} + +template class NodeBasic; + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 338e9a4a2df..c6ddd142339 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -13,35 +13,35 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. +#include "nav2_smac_planner/node_hybrid.hpp" + #include + +#include #include -#include +#include #include -#include #include -#include #include +#include #include "ompl/base/ScopedState.h" #include "ompl/base/spaces/DubinsStateSpace.h" #include "ompl/base/spaces/ReedsSheppStateSpace.h" -#include "nav2_smac_planner/node_hybrid.hpp" - using namespace std::chrono; // NOLINT namespace nav2_smac_planner { - // defining static member for all instance to share LookupTable NodeHybrid::obstacle_heuristic_lookup_table; -std::queue NodeHybrid::obstacle_heuristic_queue; double NodeHybrid::travel_distance_cost = sqrt(2); HybridMotionTable NodeHybrid::motion_table; float NodeHybrid::size_lookup = 25; LookupTable NodeHybrid::dist_heuristic_lookup_table; nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr; CostmapDownsampler NodeHybrid::downsampler; +ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -53,9 +53,7 @@ CostmapDownsampler NodeHybrid::downsampler; // http://planning.cs.uiuc.edu/node821.html // Model for ackermann style vehicle with minimum radius restriction void HybridMotionTable::initDubin( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, + unsigned int & size_x_in, unsigned int & /*size_y_in*/, unsigned int & num_angle_quantization_in, SearchInfo & search_info) { size_x = size_x_in; @@ -63,12 +61,13 @@ void HybridMotionTable::initDubin( non_straight_penalty = search_info.non_straight_penalty; cost_penalty = search_info.cost_penalty; reverse_penalty = search_info.reverse_penalty; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; // if nothing changed, no need to re-compute primitives - if (num_angle_quantization_in == num_angle_quantization && + if ( + num_angle_quantization_in == num_angle_quantization && min_turning_radius == search_info.minimum_turning_radius && - motion_model == MotionModel::DUBIN) - { + motion_model == MotionModel::DUBIN) { return; } @@ -90,8 +89,7 @@ void HybridMotionTable::initDubin( float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); // Now make sure angle is an increment of the quantized bin size // And since its based on the minimum chord, we need to make sure its always larger - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + bin_size = 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); float increments; if (angle < bin_size) { increments = 1.0f; @@ -113,8 +111,8 @@ void HybridMotionTable::initDubin( projections.clear(); projections.reserve(3); projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward - projections.emplace_back(delta_x, delta_y, increments); // Left - projections.emplace_back(delta_x, -delta_y, -increments); // Right + projections.emplace_back(delta_x, delta_y, increments); // Left + projections.emplace_back(delta_x, -delta_y, -increments); // Right // Create the correct OMPL state space state_space = std::make_unique(min_turning_radius); @@ -145,9 +143,7 @@ void HybridMotionTable::initDubin( // Same as Dubin model but now reverse is valid // See notes in Dubin for explanation void HybridMotionTable::initReedsShepp( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, + unsigned int & size_x_in, unsigned int & /*size_y_in*/, unsigned int & num_angle_quantization_in, SearchInfo & search_info) { size_x = size_x_in; @@ -155,12 +151,13 @@ void HybridMotionTable::initReedsShepp( non_straight_penalty = search_info.non_straight_penalty; cost_penalty = search_info.cost_penalty; reverse_penalty = search_info.reverse_penalty; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; // if nothing changed, no need to re-compute primitives - if (num_angle_quantization_in == num_angle_quantization && + if ( + num_angle_quantization_in == num_angle_quantization && min_turning_radius == search_info.minimum_turning_radius && - motion_model == MotionModel::REEDS_SHEPP) - { + motion_model == MotionModel::REEDS_SHEPP) { return; } @@ -170,8 +167,7 @@ void HybridMotionTable::initReedsShepp( motion_model = MotionModel::REEDS_SHEPP; float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + bin_size = 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); float increments; if (angle < bin_size) { increments = 1.0f; @@ -185,12 +181,12 @@ void HybridMotionTable::initReedsShepp( projections.clear(); projections.reserve(6); - projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward - projections.emplace_back(delta_x, delta_y, increments); // Forward + Left - projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Forward + Left + projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward - projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left - projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right + projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left + projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right // Create the correct OMPL state space state_space = std::make_unique(min_turning_radius); @@ -238,14 +234,23 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) } projection_list.emplace_back( - delta_xs[i][node_heading] + node->pose.x, - delta_ys[i][node_heading] + node->pose.y, + delta_xs[i][node_heading] + node->pose.x, delta_ys[i][node_heading] + node->pose.y, new_heading); } return projection_list; } +unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) +{ + return static_cast(floor(theta / bin_size)); +} + +float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) +{ + return bin_idx * bin_size; +} + NodeHybrid::NodeHybrid(const unsigned int index) : parent(nullptr), pose(0.0f, 0.0f, 0.0f), @@ -257,10 +262,7 @@ NodeHybrid::NodeHybrid(const unsigned int index) { } -NodeHybrid::~NodeHybrid() -{ - parent = nullptr; -} +NodeHybrid::~NodeHybrid() { parent = nullptr; } void NodeHybrid::reset() { @@ -275,17 +277,20 @@ void NodeHybrid::reset() } bool NodeHybrid::isNodeValid( - const bool & traverse_unknown, - GridCollisionChecker * collision_checker) + const bool & traverse_unknown, GridCollisionChecker * collision_checker, bool traverse_cost) { if (collision_checker->inCollision( - this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) - { + this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown)) { return false; } _cell_cost = collision_checker->getCost(); - return true; + + if (traverse_cost) { + return true; + } else { + return _cell_cost == 0.0; + } } float NodeHybrid::getTraversalCost(const NodePtr & child) @@ -293,8 +298,8 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) const float normalized_cost = child->getCost() / 252.0; if (std::isnan(normalized_cost)) { throw std::runtime_error( - "Node attempted to get traversal " - "cost without a known SE2 collision cost!"); + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); } // this is the first node @@ -302,20 +307,10 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) return NodeHybrid::travel_distance_cost; } - // Note(stevemacenski): `travel_cost_raw` at one point contained a term: - // `+ motion_table.cost_penalty * normalized_cost;` - // It has been removed, but we may want to readdress this point and determine - // the technically and theoretically correctness of that choice. I feel technically speaking - // that term has merit, but it doesn't seem to impact performance or path quality. - // W/o it lowers the travel cost, which would drive the heuristics up proportionally where I - // would expect it to plan much faster in all cases, but I only see it in some cases. Since - // this term would weight against moving to high cost zones, I would expect to see more smooth - // central motion, but I only see it in some cases, possibly because the obstacle heuristic is - // already driving the robot away from high cost areas; implicitly handling this. However, - // then I would expect that not adding it here would make it unbalanced enough that path quality - // would suffer, which I did not see in my limited experimentation, possibly due to the smoother. float travel_cost = 0.0; - float travel_cost_raw = NodeHybrid::travel_distance_cost; + float travel_cost_raw = + NodeHybrid::travel_distance_cost * + (motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost); if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { // New motion is a straight motion, no additional costs to be applied @@ -326,12 +321,12 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) travel_cost = travel_cost_raw * motion_table.non_straight_penalty; } else { // Turning motion and changing direction: penalizes wiggling - travel_cost = travel_cost_raw * - (motion_table.non_straight_penalty + motion_table.change_penalty); + travel_cost = + travel_cost_raw * (motion_table.non_straight_penalty + motion_table.change_penalty); } } - if (getMotionPrimitiveIndex() > 2) { + if (child->getMotionPrimitiveIndex() > 2) { // reverse direction travel_cost *= motion_table.reverse_penalty; } @@ -340,21 +335,18 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) } float NodeHybrid::getHeuristicCost( - const Coordinates & node_coords, - const Coordinates & goal_coords, + const Coordinates & node_coords, const Coordinates & goal_coords, const nav2_costmap_2d::Costmap2D * /*costmap*/) { - const float obstacle_heuristic = getObstacleHeuristic(node_coords, goal_coords); + const float obstacle_heuristic = + getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, dist_heuristic); } void NodeHybrid::initMotionModel( - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & num_angle_quantization, - SearchInfo & search_info) + const MotionModel & motion_model, unsigned int & size_x, unsigned int & size_y, + unsigned int & num_angle_quantization, SearchInfo & search_info) { // find the motion model selected switch (motion_model) { @@ -366,16 +358,25 @@ void NodeHybrid::initMotionModel( break; default: throw std::runtime_error( - "Invalid motion model for Hybrid A*. Please select between" - " Dubin (Ackermann forward only)," - " Reeds-Shepp (Ackermann forward and back)."); + "Invalid motion model for Hybrid A*. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); } travel_distance_cost = motion_table.projections[0]._x; } +inline float distanceHeuristic2D( + const unsigned int idx, const unsigned int size_x, const unsigned int target_x, + const unsigned int target_y) +{ + return std::hypotf( + static_cast(idx % size_x) - static_cast(target_x), + static_cast(idx / size_x) - static_cast(target_y)); +} + void NodeHybrid::resetObstacleHeuristic( - nav2_costmap_2d::Costmap2D * costmap, + nav2_costmap_2d::Costmap2D * costmap, const unsigned int & start_x, const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y) { // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up @@ -383,7 +384,7 @@ void NodeHybrid::resetObstacleHeuristic( // erosion of path quality after even modest smoothing. The error would be no more // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality std::weak_ptr ptr; - downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0); + downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0, true); downsampler.on_activate(); sampled_costmap = downsampler.downsample(2.0); @@ -391,36 +392,42 @@ void NodeHybrid::resetObstacleHeuristic( unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY(); if (obstacle_heuristic_lookup_table.size() == size) { // must reset all values - std::fill( - obstacle_heuristic_lookup_table.begin(), - obstacle_heuristic_lookup_table.end(), 0.0); + std::fill(obstacle_heuristic_lookup_table.begin(), obstacle_heuristic_lookup_table.end(), 0.0); } else { unsigned int obstacle_size = obstacle_heuristic_lookup_table.size(); obstacle_heuristic_lookup_table.resize(size, 0.0); // must reset values for non-constructed indices - std::fill_n( - obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0); + std::fill_n(obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0); } + obstacle_heuristic_queue.clear(); + obstacle_heuristic_queue.reserve( + sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY()); + // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. - std::queue q; - std::swap(obstacle_heuristic_queue, q); - obstacle_heuristic_queue.emplace( - ceil(goal_y / 2.0) * sampled_costmap->getSizeInCellsX() + ceil(goal_x / 2.0)); + const unsigned int size_x = sampled_costmap->getSizeInCellsX(); + const unsigned int goal_index = floor(goal_y / 2.0) * size_x + floor(goal_x / 2.0); + obstacle_heuristic_queue.emplace_back( + distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index); + + // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized) + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } float NodeHybrid::getObstacleHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords) + const Coordinates & node_coords, const Coordinates & goal_coords, const double & cost_penalty) { // If already expanded, return the cost - unsigned int size_x = sampled_costmap->getSizeInCellsX(); + const unsigned int size_x = sampled_costmap->getSizeInCellsX(); // Divided by 2 due to downsampled costmap. - const unsigned int start_index = ceil(node_coords.y / 2.0) * size_x + ceil(node_coords.x / 2.0); - const float & starting_cost = obstacle_heuristic_lookup_table[start_index]; - if (starting_cost > 0.0) { + const unsigned int start_y = floor(node_coords.y / 2.0); + const unsigned int start_x = floor(node_coords.x / 2.0); + const unsigned int start_index = start_y * size_x + start_x; + const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index]; + if (requested_node_cost > 0.0f) { // costs are doubled due to downsampling - return 2.0 * starting_cost; + return 2.0 * requested_node_cost; } // If not, expand until it is included. This dynamic programming ensures that @@ -428,31 +435,48 @@ float NodeHybrid::getObstacleHeuristic( // Rather than naively expanding the entire (potentially massive) map for a limited // path, we only expand to the extent required for the furthest expansion in the // search-planning request that dynamically updates during search as needed. + + // start_x and start_y have changed since last call + // we need to recompute 2D distance heuristic and reprioritize queue + for (auto & n : obstacle_heuristic_queue) { + n.first = -obstacle_heuristic_lookup_table[n.second] + + distanceHeuristic2D(n.second, size_x, start_x, start_y); + } + std::make_heap( + obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(), + ObstacleHeuristicComparator{}); + const int size_x_int = static_cast(size_x); const unsigned int size_y = sampled_costmap->getSizeInCellsY(); const float sqrt_2 = sqrt(2); - unsigned int mx, my, mx_idx, my_idx; - unsigned int idx = 0, new_idx = 0; - float last_accumulated_cost = 0.0, travel_cost = 0.0; - float heuristic_cost = 0.0, current_accumulated_cost = 0.0; - float cost = 0.0, existing_cost = 0.0; - - const std::vector neighborhood = {1, -1, // left right - size_x_int, -size_x_int, // up down - size_x_int + 1, size_x_int - 1, // upper diagonals - -size_x_int + 1, -size_x_int - 1}; // lower diagonals + float c_cost, cost, travel_cost, new_cost, existing_cost; + unsigned int idx, mx, my, mx_idx, my_idx; + unsigned int new_idx = 0; + + const std::vector neighborhood = { + 1, + -1, // left right + size_x_int, + -size_x_int, // up down + size_x_int + 1, + size_x_int - 1, // upper diagonals + -size_x_int + 1, + -size_x_int - 1}; // lower diagonals while (!obstacle_heuristic_queue.empty()) { - // get next one - idx = obstacle_heuristic_queue.front(); - obstacle_heuristic_queue.pop(); - last_accumulated_cost = obstacle_heuristic_lookup_table[idx]; - - - if (idx == start_index) { - // costs are doubled due to downsampling - return 2.0 * last_accumulated_cost; + idx = obstacle_heuristic_queue.front().second; + std::pop_heap( + obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(), + ObstacleHeuristicComparator{}); + obstacle_heuristic_queue.pop_back(); + c_cost = obstacle_heuristic_lookup_table[idx]; + if (c_cost > 0.0f) { + // cell has been processed and closed, no further cost improvements + // are mathematically possible thanks to euclidean distance heuristic consistency + continue; } + c_cost = -c_cost; + obstacle_heuristic_lookup_table[idx] = c_cost; // set a positive value to close the cell my_idx = idx / size_x; mx_idx = idx - (my_idx * size_x); @@ -460,13 +484,14 @@ float NodeHybrid::getObstacleHeuristic( // find neighbors for (unsigned int i = 0; i != neighborhood.size(); i++) { new_idx = static_cast(static_cast(idx) + neighborhood[i]); - cost = static_cast(sampled_costmap->getCost(idx)); - travel_cost = - ((i <= 3) ? 1.0 : sqrt_2) + (motion_table.cost_penalty * cost / 252.0); - current_accumulated_cost = last_accumulated_cost + travel_cost; // if neighbor path is better and non-lethal, set new cost and add to queue - if (new_idx > 0 && new_idx < size_x * size_y && cost < INSCRIBED) { + if (new_idx < size_x * size_y) { + cost = static_cast(sampled_costmap->getCost(new_idx)); + if (cost >= INSCRIBED) { + continue; + } + my = new_idx / size_x; mx = new_idx - (my * size_x); @@ -478,21 +503,34 @@ float NodeHybrid::getObstacleHeuristic( } existing_cost = obstacle_heuristic_lookup_table[new_idx]; - if (existing_cost == 0.0 || existing_cost > current_accumulated_cost) { - obstacle_heuristic_lookup_table[new_idx] = current_accumulated_cost; - obstacle_heuristic_queue.emplace(new_idx); + if (existing_cost <= 0.0f) { + travel_cost = ((i <= 3) ? 1.0f : sqrt_2) * (1.0f + (cost_penalty * cost / 252.0f)); + new_cost = c_cost + travel_cost; + if (existing_cost == 0.0f || -existing_cost > new_cost) { + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table[new_idx] = -new_cost; + obstacle_heuristic_queue.emplace_back( + new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx); + std::push_heap( + obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(), + ObstacleHeuristicComparator{}); + } } } } + + if (idx == start_index) { + break; + } } + // return requested_node_cost which has been updated by the search // costs are doubled due to downsampling - return 2.0 * obstacle_heuristic_lookup_table[start_index]; + return 2.0 * requested_node_cost; } float NodeHybrid::getDistanceHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords, + const Coordinates & node_coords, const Coordinates & goal_coords, const float & obstacle_heuristic) { // rotate and translate node_coords such that goal_coords relative is (0,0,0) @@ -509,16 +547,15 @@ float NodeHybrid::getDistanceHeuristic( const float dy = node_coords.y - goal_coords.y; double dtheta_bin = node_coords.theta - goal_coords.theta; + if (dtheta_bin < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } if (dtheta_bin > motion_table.num_angle_quantization) { dtheta_bin -= motion_table.num_angle_quantization; - } else if (dtheta_bin < 0) { - dtheta_bin += motion_table.num_angle_quantization; } Coordinates node_coords_relative( - round(dx * cos_th - dy * sin_th), - round(dx * sin_th + dy * cos_th), - round(dtheta_bin)); + round(dx * cos_th - dy * sin_th), round(dx * sin_th + dy * cos_th), round(dtheta_bin)); // Check if the relative node coordinate is within the localized window around the goal // to apply the distance heuristic. Since the lookup table is contains only the positive @@ -537,10 +574,8 @@ float NodeHybrid::getDistanceHeuristic( } const int x_pos = node_coords_relative.x + floored_size; const int y_pos = static_cast(mirrored_relative_y); - const int index = - x_pos * ceiling_size * motion_table.num_angle_quantization + - y_pos * motion_table.num_angle_quantization + - theta_pos; + const int index = x_pos * ceiling_size * motion_table.num_angle_quantization + + y_pos * motion_table.num_angle_quantization + theta_pos; motion_heuristic = dist_heuristic_lookup_table[index]; } else if (obstacle_heuristic == 0.0) { // If no obstacle heuristic value, must have some H to use @@ -559,22 +594,20 @@ float NodeHybrid::getDistanceHeuristic( } void NodeHybrid::precomputeDistanceHeuristic( - const float & lookup_table_dim, - const MotionModel & motion_model, - const unsigned int & dim_3_size, + const float & lookup_table_dim, const MotionModel & motion_model, const unsigned int & dim_3_size, const SearchInfo & search_info) { // Dubin or Reeds-Shepp shortest distances if (motion_model == MotionModel::DUBIN) { - motion_table.state_space = std::make_unique( - search_info.minimum_turning_radius); + motion_table.state_space = + std::make_unique(search_info.minimum_turning_radius); } else if (motion_model == MotionModel::REEDS_SHEPP) { - motion_table.state_space = std::make_unique( - search_info.minimum_turning_radius); + motion_table.state_space = + std::make_unique(search_info.minimum_turning_radius); } else { throw std::runtime_error( - "Node attempted to precompute distance heuristics " - "with invalid motion model!"); + "Node attempted to precompute distance heuristics " + "with invalid motion model!"); } ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); @@ -608,10 +641,8 @@ void NodeHybrid::precomputeDistanceHeuristic( } void NodeHybrid::getNeighbors( - std::function & NeighborGetter, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors) + std::function & NeighborGetter, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { unsigned int index = 0; NodePtr neighbor = nullptr; @@ -622,26 +653,56 @@ void NodeHybrid::getNeighbors( index = NodeHybrid::getIndex( static_cast(motion_projections[i]._x), static_cast(motion_projections[i]._y), - static_cast(motion_projections[i]._theta), - motion_table.size_x, motion_table.num_angle_quantization); + static_cast(motion_projections[i]._theta), motion_table.size_x, + motion_table.num_angle_quantization); if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { - // Cache the initial pose in case it was visited but valid - // don't want to disrupt continuous coordinate expansion - initial_node_coords = neighbor->pose; - neighbor->setPose( - Coordinates( - motion_projections[i]._x, - motion_projections[i]._y, - motion_projections[i]._theta)); - if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { - neighbor->setMotionPrimitiveIndex(i); - neighbors.push_back(neighbor); + if (neighbor->is_goal) { + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i); + neighbors.push_back(neighbor); + } } else { - neighbor->setPose(initial_node_coords); + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose(Coordinates( + motion_projections[i]._x, motion_projections[i]._y, motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } } } } } +bool NodeHybrid::backtracePath(CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + // goal + path.push_back(this->pose); + path.back().theta = this->continuous_angle_goal; + + NodePtr current_node = this->parent; + + while (current_node->parent && current_node->parent != current_node) { + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); + current_node = current_node->parent; + } + + // start + path.push_back(current_node->pose_start); + path.back().theta = current_node->continuous_angle_start; + + return path.size() > 0; +} + } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index b4a755dc0bb..8e54d801e7b 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include -#include -#include +#include "nav2_smac_planner/smac_planner_hybrid.hpp" + #include #include +#include +#include +#include #include "Eigen/Core" -#include "nav2_smac_planner/smac_planner_hybrid.hpp" // #define BENCHMARK_TESTING namespace nav2_smac_planner { - using namespace std::chrono; // NOLINT using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; @@ -41,14 +41,12 @@ SmacPlannerHybrid::SmacPlannerHybrid() SmacPlannerHybrid::~SmacPlannerHybrid() { - RCLCPP_INFO( - _logger, "Destroying plugin %s of type SmacPlannerHybrid", - _name.c_str()); + RCLCPP_INFO(_logger, "Destroying plugin %s of type SmacPlannerHybrid", _name.c_str()); } void SmacPlannerHybrid::configure( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, std::shared_ptr/*tf*/, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, + std::shared_ptr /*tf*/, std::shared_ptr costmap_ros) { _node = parent; @@ -60,7 +58,11 @@ void SmacPlannerHybrid::configure( _name = name; _global_frame = costmap_ros->getGlobalFrameID(); + RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", name.c_str()); + int angle_quantizations; + double analytic_expansion_max_length_m; + bool smooth_path; // General planner params nav2_util::declare_parameter_if_not_declared( @@ -82,10 +84,13 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".smooth_path", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); - node->get_parameter(name + ".minimum_turning_radius", _search_info.minimum_turning_radius); + node->get_parameter(name + ".minimum_turning_radius", _minimum_turning_radius_global_coords); nav2_util::declare_parameter_if_not_declared( node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false)); node->get_parameter(name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic); @@ -93,17 +98,25 @@ void SmacPlannerHybrid::configure( node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".reverse_penalty", _search_info.reverse_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".change_penalty", rclcpp::ParameterValue(0.15)); + node, name + ".change_penalty", rclcpp::ParameterValue(0.0)); node->get_parameter(name + ".change_penalty", _search_info.change_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.50)); + node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.2)); node->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".cost_penalty", rclcpp::ParameterValue(1.7)); + node, name + ".cost_penalty", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015)); + node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5)); node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio); + nav2_util::declare_parameter_if_not_declared( + node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0)); + node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m); + _search_info.analytic_expansion_max_length = + analytic_expansion_max_length_m / _costmap->getResolution(); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(5.0)); @@ -126,7 +139,8 @@ void SmacPlannerHybrid::configure( if (_max_iterations <= 0) { RCLCPP_INFO( - _logger, "maximum iteration selected as <= 0, " + _logger, + "maximum iteration selected as <= 0, " "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } @@ -135,12 +149,10 @@ void SmacPlannerHybrid::configure( if (!_downsample_costmap) { _downsampling_factor = 1; } - const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; _search_info.minimum_turning_radius = - _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); - _lookup_table_dim = - static_cast(_lookup_table_size) / - static_cast(_costmap->getResolution() * _downsampling_factor); + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); + _lookup_table_dim = static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); // Make sure its a whole number _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); @@ -148,8 +160,7 @@ void SmacPlannerHybrid::configure( // Make sure its an odd number if (static_cast(_lookup_table_dim) % 2 == 0) { RCLCPP_INFO( - _logger, - "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + _logger, "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", _lookup_table_dim); _lookup_table_dim += 1.0; } @@ -157,24 +168,22 @@ void SmacPlannerHybrid::configure( // Initialize collision checker _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); _collision_checker.setFootprint( - _costmap_ros->getRobotFootprint(), - _costmap_ros->getUseRadius(), + _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), findCircumscribedCost(_costmap_ros)); // Initialize A* template _a_star = std::make_unique>(_motion_model, _search_info); _a_star->initialize( - _allow_unknown, - _max_iterations, - std::numeric_limits::max(), - _lookup_table_dim, + _allow_unknown, _max_iterations, 1000, _max_planning_time, _lookup_table_dim, _angle_quantizations); // Initialize path smoother - SmootherParams params; - params.get(node, name); - _smoother = std::make_unique(params); - _smoother->initialize(minimum_turning_radius_global_coords); + if (smooth_path) { + SmootherParams params; + params.get(node, name); + _smoother = std::make_unique(params); + _smoother->initialize(_minimum_turning_radius_global_coords); + } // Initialize costmap downsampler if (_downsample_costmap && _downsampling_factor > 1) { @@ -186,18 +195,9 @@ void SmacPlannerHybrid::configure( _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); - // Setup callback for changes to parameters. - _parameters_client = std::make_shared( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface()); - - _parameter_event_sub = _parameters_client->on_parameter_event( - std::bind(&SmacPlannerHybrid::on_parameter_event_callback, this, _1)); - RCLCPP_INFO( - _logger, "Configured plugin %s of type SmacPlannerHybrid with " + _logger, + "Configured plugin %s of type SmacPlannerHybrid with " "maximum iterations %i, and %s. Using motion model: %s.", _name.c_str(), _max_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", @@ -206,31 +206,30 @@ void SmacPlannerHybrid::configure( void SmacPlannerHybrid::activate() { - RCLCPP_INFO( - _logger, "Activating plugin %s of type SmacPlannerHybrid", - _name.c_str()); + RCLCPP_INFO(_logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { _costmap_downsampler->on_activate(); } + auto node = _node.lock(); + // Add callback for dynamic parameters + _dyn_params_handler = node->add_on_set_parameters_callback( + std::bind(&SmacPlannerHybrid::dynamicParametersCallback, this, _1)); } void SmacPlannerHybrid::deactivate() { - RCLCPP_INFO( - _logger, "Deactivating plugin %s of type SmacPlannerHybrid", - _name.c_str()); + RCLCPP_INFO(_logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } + _dyn_params_handler.reset(); } void SmacPlannerHybrid::cleanup() { - RCLCPP_INFO( - _logger, "Cleaning up plugin %s of type SmacPlannerHybrid", - _name.c_str()); + RCLCPP_INFO(_logger, "Cleaning up plugin %s of type SmacPlannerHybrid", _name.c_str()); _a_star.reset(); _smoother.reset(); if (_costmap_downsampler) { @@ -241,8 +240,7 @@ void SmacPlannerHybrid::cleanup() } nav_msgs::msg::Path SmacPlannerHybrid::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -272,9 +270,27 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); - auto mapPose = getMapCoords(start.pose.position.x, start.pose.position.y, costmap); + float start_yaw = tf2::getYaw(start.pose.orientation); + if (start_yaw < 0.0) { + start_yaw += 2.0 * M_PI; + } + if (start_yaw >= 2.0 * M_PI) { + start_yaw -= 2.0 * M_PI; + } + + // std::cout << "\n\nstart.pose.position.x = " << start.pose.position.x + // << ", start.pose.position.y = " << start.pose.position.y << "\n\n" + // << std::endl; + // std::cout << "\n\ngoal.pose.position.x = " << goal.pose.position.x + // << ", goal.pose.position.y = " << goal.pose.position.y << "\n\n" + // << std::endl; - _a_star->setStart(mx, my, orientation_bin_id, mapPose.position.x, mapPose.position.y); + auto map_pose_start = getMapCoords(start.pose.position.x, start.pose.position.y, costmap); + // std::cout << "\n\nSTART: map_pose_start.position.x = " << map_pose_start.position.x + // << ", map_pose_start.position.y = " << map_pose_start.position.y << "\n\n" + // << std::endl; + _a_star->setStart( + mx, my, orientation_bin_id, map_pose_start.position.x, map_pose_start.position.y, start_yaw); // Set goal point, in A* bin search coordinates costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); @@ -287,10 +303,21 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( orientation_bin -= static_cast(_angle_quantizations); } orientation_bin_id = static_cast(floor(orientation_bin)); - - mapPose = getMapCoords(goal.pose.position.x, goal.pose.position.y, costmap); - _a_star->setGoal(mx, my, orientation_bin_id, mapPose.position.x, mapPose.position.y); + float goal_yaw = tf2::getYaw(goal.pose.orientation); + if (goal_yaw < 0.0) { + goal_yaw += 2.0 * M_PI; + } + if (goal_yaw >= 2.0 * M_PI) { + goal_yaw -= 2.0 * M_PI; + } + + auto map_pose_goal = getMapCoords(goal.pose.position.x, goal.pose.position.y, costmap); + // std::cout << "\n\nGOAL: map_pose_goal.position.x = " << map_pose_goal.position.x + // << ", map_pose_goal.position.y = " << map_pose_goal.position.y << "\n\n" + // << std::endl; + _a_star->setGoal( + mx, my, orientation_bin_id, map_pose_goal.position.x, map_pose_goal.position.y, goal_yaw); // Setup message nav_msgs::msg::Path plan; @@ -309,7 +336,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0.0)) { + if (!_a_star->createPath(path, num_iterations, 5.0)) { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -322,21 +349,28 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); + RCLCPP_WARN(_logger, "%s: failed to create plan, %s.", _name.c_str(), error.c_str()); return plan; } + // std::cout << "path front -> x: " << path.front().x << "y: " << path.front().y + // << "yaw: " << path.front().theta << std::endl; + // Convert to world coordinates plan.poses.reserve(path.size()); for (int i = path.size() - 1; i >= 0; --i) { pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); - pose.pose.orientation = getWorldOrientation(path[i].theta, _angle_bin_size); + pose.pose.orientation = getWorldOrientation(path[i].theta); plan.poses.push_back(pose); } + // std::cout << "plan back -> x: " << plan.poses.back().pose.position.x + // << "y: " << plan.poses.back().pose.position.y + // << "yaw: " << tf2::getYaw(plan.poses.back().pose.orientation) << std::endl; + + // std::cout << "original -> x: " << goal.pose.position.x << "y: " << goal.pose.position.y + // << "yaw: " << tf2::getYaw(goal.pose.orientation) << std::endl; + // Publish raw path for debug if (_raw_plan_publisher->get_subscription_count() > 0) { _raw_plan_publisher->publish(plan); @@ -348,28 +382,29 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( 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; + std::cout << "It took " << time_span.count() * 1000 << " milliseconds with " << num_iterations + << " iterations." << std::endl; #endif // Smooth plan - if (num_iterations > 1 && plan.poses.size() > 6) { + 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; + std::cout << "It took " << time_span2.count() * 1000 << " milliseconds to smooth path." + << std::endl; #endif return plan; } -void SmacPlannerHybrid::on_parameter_event_callback( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +rcl_interfaces::msg::SetParametersResult SmacPlannerHybrid::dynamicParametersCallback( + std::vector parameters) { + rcl_interfaces::msg::SetParametersResult result; std::lock_guard lock_reinit(_mutex); bool reinit_collision_checker = false; @@ -377,73 +412,86 @@ void SmacPlannerHybrid::on_parameter_event_callback( bool reinit_downsampler = false; bool reinit_smoother = false; - for (auto & changed_parameter : event->changed_parameters) { - const auto & type = changed_parameter.value.type; - const auto & name = changed_parameter.name; - const auto & value = changed_parameter.value; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); if (type == ParameterType::PARAMETER_DOUBLE) { if (name == _name + ".max_planning_time") { - _max_planning_time = value.double_value; + reinit_a_star = true; + _max_planning_time = parameter.as_double(); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; - _lookup_table_size = value.double_value; + _lookup_table_size = parameter.as_double(); } else if (name == _name + ".minimum_turning_radius") { reinit_a_star = true; - reinit_smoother = true; - _search_info.minimum_turning_radius = static_cast(value.double_value); + if (_smoother) { + reinit_smoother = true; + } + _minimum_turning_radius_global_coords = static_cast(parameter.as_double()); } else if (name == _name + ".reverse_penalty") { reinit_a_star = true; - _search_info.reverse_penalty = static_cast(value.double_value); + _search_info.reverse_penalty = static_cast(parameter.as_double()); } else if (name == _name + ".change_penalty") { reinit_a_star = true; - _search_info.change_penalty = static_cast(value.double_value); + _search_info.change_penalty = static_cast(parameter.as_double()); } else if (name == _name + ".non_straight_penalty") { reinit_a_star = true; - _search_info.non_straight_penalty = static_cast(value.double_value); + _search_info.non_straight_penalty = static_cast(parameter.as_double()); } else if (name == _name + ".cost_penalty") { reinit_a_star = true; - _search_info.cost_penalty = static_cast(value.double_value); + _search_info.cost_penalty = static_cast(parameter.as_double()); } else if (name == _name + ".analytic_expansion_ratio") { reinit_a_star = true; - _search_info.analytic_expansion_ratio = static_cast(value.double_value); + _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); + } else if (name == _name + ".analytic_expansion_max_length") { + reinit_a_star = true; + _search_info.analytic_expansion_max_length = + static_cast(parameter.as_double()) / _costmap->getResolution(); } } else if (type == ParameterType::PARAMETER_BOOL) { if (name == _name + ".downsample_costmap") { reinit_downsampler = true; - _downsample_costmap = value.bool_value; + _downsample_costmap = parameter.as_bool(); } else if (name == _name + ".allow_unknown") { reinit_a_star = true; - _allow_unknown = value.bool_value; + _allow_unknown = parameter.as_bool(); } else if (name == _name + ".cache_obstacle_heuristic") { reinit_a_star = true; - _search_info.cache_obstacle_heuristic = value.bool_value; + _search_info.cache_obstacle_heuristic = parameter.as_bool(); + } else if (name == _name + ".smooth_path") { + if (parameter.as_bool()) { + reinit_smoother = true; + } else { + _smoother.reset(); + } } } else if (type == ParameterType::PARAMETER_INTEGER) { if (name == _name + ".downsampling_factor") { reinit_a_star = true; reinit_downsampler = true; - _downsampling_factor = value.integer_value; + _downsampling_factor = parameter.as_int(); } else if (name == _name + ".max_iterations") { reinit_a_star = true; - _max_iterations = value.integer_value; + _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { RCLCPP_INFO( - _logger, "maximum iteration selected as <= 0, " + _logger, + "maximum iteration selected as <= 0, " "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; - int angle_quantizations = value.integer_value; + int angle_quantizations = parameter.as_int(); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".motion_model_for_search") { reinit_a_star = true; - _motion_model = fromString(value.string_value); + _motion_model = fromString(parameter.as_string()); if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, @@ -461,12 +509,10 @@ void SmacPlannerHybrid::on_parameter_event_callback( if (!_downsample_costmap) { _downsampling_factor = 1; } - const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; _search_info.minimum_turning_radius = - _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); - _lookup_table_dim = - static_cast(_lookup_table_size) / - static_cast(_costmap->getResolution() * _downsampling_factor); + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); + _lookup_table_dim = static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); // Make sure its a whole number _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); @@ -474,8 +520,7 @@ void SmacPlannerHybrid::on_parameter_event_callback( // Make sure its an odd number if (static_cast(_lookup_table_dim) % 2 == 0) { RCLCPP_INFO( - _logger, - "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + _logger, "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", _lookup_table_dim); _lookup_table_dim += 1.0; } @@ -484,10 +529,7 @@ void SmacPlannerHybrid::on_parameter_event_callback( if (reinit_a_star) { _a_star = std::make_unique>(_motion_model, _search_info); _a_star->initialize( - _allow_unknown, - _max_iterations, - std::numeric_limits::max(), - _lookup_table_dim, + _allow_unknown, _max_iterations, 1000, _max_planning_time, _lookup_table_dim, _angle_quantizations); } @@ -506,8 +548,7 @@ void SmacPlannerHybrid::on_parameter_event_callback( if (reinit_collision_checker) { _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); _collision_checker.setFootprint( - _costmap_ros->getRobotFootprint(), - _costmap_ros->getUseRadius(), + _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), findCircumscribedCost(_costmap_ros)); } @@ -517,9 +558,11 @@ void SmacPlannerHybrid::on_parameter_event_callback( SmootherParams params; params.get(node, _name); _smoother = std::make_unique(params); - _smoother->initialize(minimum_turning_radius_global_coords); + _smoother->initialize(_minimum_turning_radius_global_coords); } } + result.successful = true; + return result; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp new file mode 100644 index 00000000000..38def6eec83 --- /dev/null +++ b/nav2_smac_planner/src/smoother.cpp @@ -0,0 +1,486 @@ +// 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 "nav2_smac_planner/smoother.hpp" + +#include +#include + +#include +#include + +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_; +} + +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) +{ + refinement_ctr_ = 0; + 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(); + + // 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 != UNKNOWN) { + 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; + } + + // Lets 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_ < 4) { + 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 existance 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 existance 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_length < min_length && + !boundary_expansions[idx].in_collision && boundary_expansions[idx].path_end_idx > 0.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) { + 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