diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index 179bb55186c21..db48154ffa214 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -77,6 +77,7 @@ None | Parameter | Type | Description | | --------------------------- | ------ | ------------------------------------------------------- | +| `search_method` | string | method of searching, start to goal or vice versa | | `only_behind_solutions` | bool | whether restricting the solutions to be behind the goal | | `use_back` | bool | whether using backward trajectory | | `adapt_expansion_distance` | bool | if true, adapt expansion distance based on environment | diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml index cadce152febc2..6ea99b2c0c061 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -31,6 +31,7 @@ # -- A* search Configurations -- astar: + search_method: "forward" # options: forward, backward only_behind_solutions: false use_back: true adapt_expansion_distance: true diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json index 92a2274347371..4494878849897 100644 --- a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json +++ b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json @@ -120,6 +120,12 @@ "astar": { "type": "object", "properties": { + "search_method": { + "type": "string", + "enum": ["forward", "backward"], + "default": "forward", + "description": "Search method to use, options: forward, backward." + }, "only_behind_solutions": { "type": "boolean", "default": false, @@ -130,6 +136,11 @@ "default": true, "description": "Allow reverse motion in A* search." }, + "adapt_expansion_distance": { + "type": "boolean", + "default": true, + "description": "Allow varying A* expansion distance based on space configuration." + }, "expansion_distance": { "type": "number", "default": 0.5, @@ -143,7 +154,12 @@ "smoothness_weight": { "type": "number", "default": 0.5, - "description": "Weight for the smoothness heuristic in A* search." + "description": "Weight for the smoothness (change in curvature) in A* search." + }, + "obstacle_distance_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for distance to obstacle in A* search." } }, "required": [ diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index cbf835df3b6c6..911144f3796e5 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -164,6 +164,7 @@ class AbstractPlanningAlgorithm const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } + double getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const; virtual ~AbstractPlanningAlgorithm() {} diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index cbdbb9d4c3fb3..8ac7c60e7ca4f 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -41,6 +41,7 @@ enum class NodeStatus : uint8_t { None, Open, Closed }; struct AstarParam { // base configs + std::string search_method; bool only_behind_solutions; // solutions should be behind the goal bool use_back; // backward search bool adapt_expansion_distance; @@ -99,6 +100,7 @@ class AstarSearch : public AbstractPlanningAlgorithm : AstarSearch( planner_common_param, collision_vehicle_shape, AstarParam{ + node.declare_parameter("astar.search_method"), node.declare_parameter("astar.only_behind_solutions"), node.declare_parameter("astar.use_back"), node.declare_parameter("astar.adapt_expansion_distance"), @@ -125,7 +127,7 @@ class AstarSearch : public AbstractPlanningAlgorithm void expandNodes(AstarNode & current_node, const bool is_back = false); void resetData(); void setPath(const AstarNode & goal); - bool setStartNode(); + void setStartNode(); double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; Pose node2pose(const AstarNode & node) const; @@ -156,6 +158,7 @@ class AstarSearch : public AbstractPlanningAlgorithm double avg_turning_radius_; double min_expansion_dist_; double max_expansion_dist_; + bool is_backward_search_; // the following constexpr values were found to be best by trial and error, through multiple // tests, and are not expected to be changed regularly, therefore they were not made into ros diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 40b16f71d5fe7..1c04fd65447b2 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -111,6 +111,7 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) auto pyAstarParam = py::class_(p, "AstarParam", py::dynamic_attr()) .def(py::init<>()) + .def_readwrite("search_method", &freespace_planning_algorithms::AstarParam::search_method) .def_readwrite( "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions) .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index 28c505fbaae02..b4af9ed87a3b9 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -44,6 +44,7 @@ # -- A* search Configurations -- astar_param = fp.AstarParam() +astar_param.search_method = "forward" astar_param.only_behind_solutions = False astar_param.use_back = True astar_param.adapt_expansion_distance = True diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 380eafb745062..97fd93e31d5cd 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -138,6 +138,16 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution); } +double AbstractPlanningAlgorithm::getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const +{ + const auto local_pose = global2local(costmap_, pose); + const auto index = pose2index(costmap_, local_pose, planner_common_param_.theta_size); + if (indexToId(index) >= static_cast(edt_map_.size())) { + return std::numeric_limits::max(); + } + return getObstacleEDT(index); +} + void AbstractPlanningAlgorithm::computeEDTMap() { const int height = costmap_.info.height; diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index fe87011273c1b..b0be1da75caec 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -29,6 +29,7 @@ #include #endif +#include #include namespace autoware::freespace_planning_algorithms @@ -83,6 +84,8 @@ AstarSearch::AstarSearch( avg_turning_radius_ = kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering); + is_backward_search_ = astar_param_.search_method == "backward"; + min_expansion_dist_ = astar_param_.expansion_distance; max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; } @@ -104,15 +107,15 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) start_pose_ = global2local(costmap_, start_pose); goal_pose_ = global2local(costmap_, goal_pose); - if (detectCollision(goal_pose_)) { - throw std::logic_error("Invalid goal pose"); + if (detectCollision(start_pose_) || detectCollision(goal_pose_)) { + throw std::logic_error("Invalid start or goal pose"); } + if (is_backward_search_) std::swap(start_pose_, goal_pose_); + setCollisionFreeDistanceMap(); - if (!setStartNode()) { - throw std::logic_error("Invalid start pose"); - } + setStartNode(); if (!search()) { throw std::logic_error("HA* failed to find path to goal"); @@ -174,12 +177,9 @@ void AstarSearch::setCollisionFreeDistanceMap() } } -bool AstarSearch::setStartNode() +void AstarSearch::setStartNode() { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); - - if (detectCollision(index)) return false; - // Set start node AstarNode * start_node = &graph_[getKey(index)]; start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); @@ -191,8 +191,6 @@ bool AstarSearch::setStartNode() // Push start node to openlist openlist_.push(start_node); - - return true; } double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const @@ -242,7 +240,8 @@ bool AstarSearch::search() void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) { const auto current_pose = node2pose(current_node); - double distance = getExpansionDistance(current_node) * (is_back ? -1.0 : 1.0); + const double direction = (is_back == is_backward_search_) ? 1.0 : -1.0; + const double distance = getExpansionDistance(current_node) * direction; int steering_index = -1 * planner_common_param_.turning_steps; for (; steering_index <= planner_common_param_.turning_steps; ++steering_index) { // skip expansion back to parent @@ -333,46 +332,65 @@ void AstarSearch::setPath(const AstarNode & goal_node) header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); header.frame_id = costmap_.header.frame_id; - waypoints_.header = header; - waypoints_.waypoints.clear(); - // From the goal node to the start node const AstarNode * node = &goal_node; + std::vector waypoints; + geometry_msgs::msg::PoseStamped pose; pose.header = header; - const auto interpolate = [this, &pose](const AstarNode & node) { + const auto interpolate = [this, &waypoints, &pose](const AstarNode & node) { if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return; const auto parent_pose = node2pose(*node.parent); const double distance_2d = calcDistance2d(node2pose(node), parent_pose); const int n = static_cast(distance_2d / min_expansion_dist_); for (int i = 1; i < n; ++i) { - const double dist = ((distance_2d * i) / n) * (node.is_back ? -1.0 : 1.0); + const double dist = + ((distance_2d * i) / n) * (node.is_back == is_backward_search_ ? 1.0 : -1.0); const double steering = node.steering_index * steering_resolution_; const auto local_pose = kinematic_bicycle_model::getPose( parent_pose, collision_vehicle_shape_.base_length, steering, dist); pose.pose = local2global(costmap_, local_pose); - waypoints_.waypoints.push_back({pose, node.is_back}); + waypoints.push_back({pose, node.is_back}); } }; // push astar nodes poses while (node != nullptr) { pose.pose = local2global(costmap_, node2pose(*node)); - waypoints_.waypoints.push_back({pose, node->is_back}); + waypoints.push_back({pose, node->is_back}); interpolate(*node); // To the next node node = node->parent; } - // Reverse the vector to be start to goal order - std::reverse(waypoints_.waypoints.begin(), waypoints_.waypoints.end()); + if (waypoints.empty()) return; - // Update first point direction - if (waypoints_.waypoints.size() > 1) { - waypoints_.waypoints.at(0).is_back = waypoints_.waypoints.at(1).is_back; + if (waypoints.size() > 1) waypoints.back().is_back = waypoints.rbegin()[1].is_back; + + if (!is_backward_search_) { + // Reverse the vector to be start to goal order + std::reverse(waypoints.begin(), waypoints.end()); + } + + waypoints_.header = header; + waypoints_.waypoints.clear(); + + for (size_t i = 0; i < waypoints.size() - 1; ++i) { + const auto & current = waypoints[i]; + const auto & next = waypoints[i + 1]; + + waypoints_.waypoints.push_back(current); + + if (current.is_back != next.is_back) { + waypoints_.waypoints.push_back( + {is_backward_search_ ? next.pose : current.pose, + is_backward_search_ ? current.is_back : next.is_back}); + } } + + waypoints_.waypoints.push_back(waypoints.back()); } bool AstarSearch::isGoal(const AstarNode & node) const diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 7284a8dcffc44..82806bb500b0e 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -209,6 +209,7 @@ std::unique_ptr configure_astar(bool use_multi) } // configure astar param + const std::string search_method = "forward"; const bool only_behind_solutions = false; const bool use_back = true; const bool adapt_expansion_distance = true; @@ -217,8 +218,8 @@ std::unique_ptr configure_astar(bool use_multi) const double smoothness_weight = 0.5; const double obstacle_distance_weight = 1.7; const auto astar_param = fpa::AstarParam{ - only_behind_solutions, use_back, adapt_expansion_distance, expansion_distance, - distance_heuristic_weight, smoothness_weight, obstacle_distance_weight}; + search_method, only_behind_solutions, use_back, adapt_expansion_distance, + expansion_distance, distance_heuristic_weight, smoothness_weight, obstacle_distance_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index d3d23ae69dc1a..b40214bf2e89e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -106,6 +106,7 @@ obstacle_threshold: 30 # -- A* search Configurations -- astar: + search_method: "forward" # options: forward, backward only_behind_solutions: false use_back: false distance_heuristic_weight: 1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 805e91f739a6a..9f146f0ad73d0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -235,6 +235,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) // freespace parking astar { const std::string ns = base_ns + "pull_over.freespace_parking.astar."; + p.astar_parameters.search_method = node->declare_parameter(ns + "search_method"); p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -623,6 +624,7 @@ void GoalPlannerModuleManager::updateModuleParams( // freespace parking astar { const std::string ns = base_ns + "pull_over.freespace_parking.astar."; + updateParam(parameters, ns + "search_method", p->astar_parameters.search_method); updateParam( parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index 851e96f7a265c..499f713a3d65b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -77,6 +77,7 @@ obstacle_threshold: 30 # -- A* search Configurations -- astar: + search_method: "forward" # options: forward, backward only_behind_solutions: false use_back: false distance_heuristic_weight: 1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 23817ee081501..193f88d212842 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -167,6 +167,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) // freespace planner astar { const std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.search_method = node->declare_parameter(ns + "search_method"); p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -501,6 +502,7 @@ void StartPlannerModuleManager::updateModuleParams( { const std::string ns = "start_planner.freespace_planner.astar."; + updateParam(parameters, ns + "search_method", p->astar_parameters.search_method); updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); updateParam( parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions);