From a3b38651c622e63f441efa392c97b60833c97087 Mon Sep 17 00:00:00 2001 From: Nisarg Date: Thu, 30 Oct 2025 18:00:27 +0900 Subject: [PATCH] code --- nav2_smac_planner/CMakeLists.txt | 4 ++ .../include/nav2_smac_planner/a_star.hpp | 16 +++++++ .../include/nav2_smac_planner/types.hpp | 10 +++++ nav2_smac_planner/src/a_star.cpp | 44 +++++++++++++++++++ nav2_smac_planner/src/analytic_expansion.cpp | 21 +++++++++ nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 +++ nav2_smac_planner/src/types.cpp | 33 ++++++++++++++ .../include/nav2_util/geometry_utils.hpp | 6 +-- 8 files changed, 137 insertions(+), 3 deletions(-) create mode 100644 nav2_smac_planner/src/types.cpp diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 08563b3df41..1895d5232ab 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(nav2_smac_planner) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(angles REQUIRED) @@ -43,6 +46,7 @@ add_library(${library_name}_common SHARED src/node_hybrid.cpp src/node_lattice.cpp src/smoother.cpp + src/types.cpp ) # Add GenerateExportHeader support for symbol visibility, as we are using # static members we need to explicitly export them on Windows, as diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 631f4bee0b7..dd3a5433b40 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -125,6 +125,15 @@ class AStarAlgorithm */ void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Used to limit the planner to explore ahead of the specified pose + * @param search_bounds the pose beyoind which we want to limit the planner + * @param start_point the start point + * @param allow_goal_overshoot enable/disable this feature + */ + void setSearchBounds(const geometry_msgs::msg::Pose& search_bounds, const geometry_msgs::msg::Point& start_point, bool allow_goal_overshoot); + /** * @brief Set the goal for planning, as a node index * @param mx The node X index of the goal @@ -270,6 +279,13 @@ class AStarAlgorithm inline void populateExpansionsLog( const NodePtr & node, std::vector> * expansions_log); + /** + * @brief check the position of a node, with respect to a pose. If it is behind the pose then return true + * @param node the node which we want to check + * @param pose the pose relative to which we want to check the position of the node + */ + bool isBehindPose(const NodePtr& node, const geometry_msgs::msg::Pose& pose); + bool _traverse_unknown; bool _is_initialized; int _max_iterations; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index 3462437c8a2..ff360108e1c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -51,6 +51,16 @@ struct SearchInfo bool allow_primitive_interpolation{false}; bool downsample_obstacle_heuristic{true}; bool use_quadratic_cost_penalty{false}; + bool allow_goal_overshoot{false}; + void setStart(const geometry_msgs::msg::Point& start); + geometry_msgs::msg::Pose getSearchBound(); + void setSearchBound(const geometry_msgs::msg::Pose& search_bound); + bool isStartBehindSearchBounds(); + + private: + geometry_msgs::msg::Point _start_pose; + geometry_msgs::msg::Pose _search_bound; + std::optional is_start_behind_goal; }; /** diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 6e4b8d00d3b..566109336e0 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -102,6 +102,26 @@ void AStarAlgorithm::initialize( _motion_model, _search_info, _traverse_unknown, _dim3_size); } +template<> +bool AStarAlgorithm::isBehindPose( + const NodePtr & node, + const geometry_msgs::msg::Pose & pose) +{ + const Node2D::Coordinates node_coords = node->getCoords(node->getIndex()); + const geometry_msgs::msg::Pose node_in_world_frame = Utils::getWorldCoords(node_coords.x, node_coords.y, _costmap); + return Utils::isBehindPose(node_in_world_frame.position, pose); +} + +template +bool AStarAlgorithm::isBehindPose( + const NodePtr & node, + const geometry_msgs::msg::Pose & pose) +{ + typename NodeT::Coordinates node_coords = node->pose; + const geometry_msgs::msg::Pose node_in_world_frame = Utils::getWorldCoords(node_coords.x, node_coords.y, _costmap); + return Utils::isBehindPose(node_in_world_frame.position, pose); +} + template void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) { @@ -120,6 +140,15 @@ void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision _expander->setCollisionChecker(_collision_checker); } +template +void AStarAlgorithm::setSearchBounds(const geometry_msgs::msg::Pose& search_bounds, const geometry_msgs::msg::Point& start_point, bool allow_goal_overshoot) +{ + _search_info.setSearchBound(search_bounds); + _search_info.setStart(start_point); + _search_info.allow_goal_overshoot = allow_goal_overshoot; + _expander->setSearchBounds(search_bounds, start_point, allow_goal_overshoot); +} + template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const uint64_t & index) @@ -367,6 +396,7 @@ bool AStarAlgorithm::createPath( NeighborIterator neighbor_iterator; int analytic_iterations = 0; int closest_distance = std::numeric_limits::max(); + const bool is_start_behind_goal = _search_info.isStartBehindSearchBounds(); // Given an index, return a node ptr reference if its collision-free and valid const uint64_t max_index = static_cast(getSizeX()) * @@ -379,6 +409,14 @@ bool AStarAlgorithm::createPath( return false; } + if (!_search_info.allow_goal_overshoot){ + auto iter = _graph.find(index); + if (iter != _graph.end()) { + if (isBehindPose(&(iter->second), _search_info.getSearchBound()) != is_start_behind_goal){ + return false; + } + } + } neighbor_rtn = addToGraph(index); return true; }; @@ -446,6 +484,12 @@ bool AStarAlgorithm::createPath( { neighbor = *neighbor_iterator; + if (!_search_info.allow_goal_overshoot) { + if ((isBehindPose(neighbor, _search_info.getSearchBound())) != is_start_behind_goal) { + continue; + } + } + // 4.1) Compute the cost to go to this node g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 13df5feb6af..138773e1467 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -42,6 +42,15 @@ void AnalyticExpansion::setCollisionChecker( _collision_checker = collision_checker; } +template +void AnalyticExpansion::setSearchBounds( + const geometry_msgs::msg::Pose &search_bounds, const geometry_msgs::msg::Point& start_point, bool allow_goal_overshoot) +{ + _search_info.setSearchBound(search_bounds); + _search_info.setStart(start_point); + _search_info.allow_goal_overshoot = allow_goal_overshoot; +} + template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( const NodePtr & current_node, @@ -204,6 +213,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -221,6 +232,16 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansioninterpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); + + if (!_search_info.allow_goal_overshoot){ + geometry_msgs::msg::Pose node_in_world_frame = Utils::getWorldCoords(reals[0], reals[1], _collision_checker->getCostmap()); + const bool is_node_behind_goal = Utils::isBehindPose(node_in_world_frame.position, _search_info.getSearchBound()); + if (is_node_behind_goal != is_start_behind_goal){ // not equal means not on the same side + failure = true; + break; + } + } + // 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; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 07a30839374..560307cbaff 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -13,6 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. +#include #include #include #include @@ -75,6 +76,10 @@ void SmacPlannerHybrid::configure( node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + nav2::declare_parameter_if_not_declared( + node, name + ".allow_goal_overshoot", rclcpp::ParameterValue(false)); + nav2::get_parameter(name + ".allow_goal_overshoot", _search_info.allow_goal_overshoot) + nav2::declare_parameter_if_not_declared( node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72)); node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); @@ -400,6 +405,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _costmap_ros->getUseRadius(), findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); + _a_star->setSearchBounds(goal.pose, start.pose.position, _search_info.allow_goal_overshoot); // Set starting point, in A* bin search coordinates float mx_start, my_start, mx_goal, my_goal; diff --git a/nav2_smac_planner/src/types.cpp b/nav2_smac_planner/src/types.cpp new file mode 100644 index 00000000000..5bba44599c1 --- /dev/null +++ b/nav2_smac_planner/src/types.cpp @@ -0,0 +1,33 @@ +#include +#include "geometry_msgs/msg/PoseStamped.hpp" +#include + +namespace smac_planner +{ + +void SearchInfo::setStart(const geometry_msgs::msg::Point& start){ + _start_pose = start; + is_start_behind_goal.reset(); +} + +geometry_msgs::msg::Pose SearchInfo::getSearchBound(){ + return _search_bound; +} + +void SearchInfo::setSearchBound(const geometry_msgs::msg::Pose& search_bound){ + _search_bound = search_bound; + is_start_behind_goal.reset(); +} + +bool SearchInfo::isStartBehindSearchBounds(){ + + if (is_start_behind_goal){ + return *is_start_behind_goal; + } + else{ + is_start_behind_goal = smac_planner::Utils::isBehindPose(_start_pose, _search_bound); + return *is_start_behind_goal; + } +} + +} // namespace smac_planner \ No newline at end of file diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index f4cf4078042..9ff39444ddb 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -44,7 +44,7 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) } /** - * @brief Get the euclidean distance between 2 geometry_msgs::Points + * @brief Get the euclidean distance between 2 geometry_msgs::msg::Points * @param pos1 First point * @param pos1 Second point * @param is_3d True if a true L2 distance is desired (default false) @@ -67,7 +67,7 @@ inline double euclidean_distance( } /** - * @brief Get the L2 distance between 2 geometry_msgs::Poses + * @brief Get the L2 distance between 2 geometry_msgs::msg::Poses * @param pos1 First pose * @param pos1 Second pose * @param is_3d True if a true L2 distance is desired (default false) @@ -90,7 +90,7 @@ inline double euclidean_distance( } /** - * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped + * @brief Get the L2 distance between 2 geometry_msgs::msg::PoseStamped * @param pos1 First pose * @param pos1 Second pose * @param is_3d True if a true L2 distance is desired (default false)