diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index dfaeba953aa..af2393b417a 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -27,6 +27,7 @@ #include "nav2_smoother/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/collision_checker.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_ros_common/node_utils.hpp" #include "nav_msgs/msg/path.hpp" @@ -94,6 +95,7 @@ class SimpleSmoother : public nav2_core::Smoother * @param costmap Pointer to minimal costmap * @param max_time Maximum time to compute, stop early if over limit */ + void smoothImpl( nav_msgs::msg::Path & path, bool & reversing_segment, @@ -124,6 +126,9 @@ class SimpleSmoother : public nav2_core::Smoother int max_its_, refinement_ctr_, refinement_num_; bool do_refinement_, enforce_path_inversion_; std::shared_ptr costmap_sub_; + //member variables added to store foorprint sub and collision checker + std::shared_ptr footprint_sub_; + std::shared_ptr collision_checker_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; }; diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 5179784f83a..5adc8d8b199 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -28,34 +28,46 @@ void SimpleSmoother::configure( const nav2::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_sub, - std::shared_ptr/*footprint_sub*/) + std::shared_ptr footprint_sub) { - costmap_sub_ = costmap_sub; - auto node = parent.lock(); logger_ = node->get_logger(); - declare_parameter_if_not_declared( - node, name + ".tolerance", rclcpp::ParameterValue(1e-10)); - declare_parameter_if_not_declared( - node, name + ".max_its", rclcpp::ParameterValue(1000)); - declare_parameter_if_not_declared( - node, name + ".w_data", rclcpp::ParameterValue(0.2)); - declare_parameter_if_not_declared( - node, name + ".w_smooth", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, name + ".do_refinement", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, name + ".refinement_num", rclcpp::ParameterValue(2)); - declare_parameter_if_not_declared( - node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); + // Store subscribers + costmap_sub_ = costmap_sub; + footprint_sub_ = footprint_sub; + // Initialize the collision checker + collision_checker_ = std::make_shared( + costmap_sub_, footprint_sub_, node->get_node_topics_interface()); + + // Get parameters + nav2::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(1e-10)); node->get_parameter(name + ".tolerance", tolerance_); + + nav2::declare_parameter_if_not_declared( + node, name + ".max_its", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_its", max_its_); + + nav2::declare_parameter_if_not_declared( + node, name + ".w_data", rclcpp::ParameterValue(0.2)); node->get_parameter(name + ".w_data", data_w_); + + nav2::declare_parameter_if_not_declared( + node, name + ".w_smooth", rclcpp::ParameterValue(0.3)); node->get_parameter(name + ".w_smooth", smooth_w_); + + nav2::declare_parameter_if_not_declared( + node, name + ".do_refinement", rclcpp::ParameterValue(true)); node->get_parameter(name + ".do_refinement", do_refinement_); + + nav2::declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); node->get_parameter(name + ".refinement_num", refinement_num_); + + nav2::declare_parameter_if_not_declared( + node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); } @@ -126,6 +138,7 @@ void SimpleSmoother::smoothImpl( nav_msgs::msg::Path new_path = path; nav_msgs::msg::Path last_path = path; + const nav_msgs::msg::Path original_path = path; //new var while (change >= tolerance_) { its += 1; @@ -199,6 +212,30 @@ void SimpleSmoother::smoothImpl( } updateApproximatePathOrientations(new_path, reversing_segment); + + /* *********************************************************************** + ************** START: NEW FOOTPRINT COLLISION CHECKING ***************** + ************************************************************************/ + //After smoothing,we perform a full footprint collision check on the result. + for (const auto & pose : new_path.poses) { + const double cost = collision_checker_->footprintCostAtPose( + pose.pose.position.x, + pose.pose.position.y, + tf2::getYaw(pose.pose.orientation)); + + //If the footprint is in collision, reject the new path and keep the original. + if (cost > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION) { + RCLCPP_WARN( + logger_, + "Smoother generated a path with a footprint in collision. " + "Reverting to the original path."); + path = original_path; + return; + } + } + /*********************************************************************** + **************** END: NEW FOOTPRINT COLLISION CHECKING **************** + ***********************************************************************/ path = new_path; }