From 357d93cb5906531890770201a739dbb6f4aee152 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 29 Jul 2024 15:25:13 +0900 Subject: [PATCH] refactor(freespace_planning_algorithm): refactor and improve astar search (#8068) * refactor freespace planning algorithms Signed-off-by: mohammad alqudah * fix error Signed-off-by: mohammad alqudah * use vector instead of map for a-star node graph Signed-off-by: mohammad alqudah * remove unnecessary parameters Signed-off-by: mohammad alqudah * precompute average turning radius Signed-off-by: mohammad alqudah * add threshold for minimum distance between direction changes Signed-off-by: mohammad alqudah * apply curvature weight and change in curvature weight Signed-off-by: mohammad alqudah * store total cost instead of heuristic cost Signed-off-by: mohammad alqudah * fix reverse weight application Signed-off-by: mohammad alqudah * fix parameter description in README Signed-off-by: mohammad alqudah * fix formats Signed-off-by: mohammad alqudah * add missing include Signed-off-by: mohammad alqudah * refactor functions Signed-off-by: mohammad alqudah * precompute number of margin cells to reduce out of range vertices check necessity Signed-off-by: mohammad alqudah * add reset data function Signed-off-by: mohammad alqudah * add member function set() to AstarNode struct Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * minor refactor Signed-off-by: mohammad alqudah * ensure expansion distance is larger than grid cell diagonal Signed-off-by: mohammad alqudah * compute collision free distance to goal map Signed-off-by: mohammad alqudah * minor refactor Signed-off-by: mohammad alqudah * fix expansion cost function Signed-off-by: mohammad alqudah * set distance map before setting start node Signed-off-by: mohammad alqudah * minor fix Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * change default parameter values Signed-off-by: mohammad alqudah * rename parameter Signed-off-by: mohammad alqudah * fix rrtstar obstacle check Signed-off-by: mohammad alqudah * remove redundant return statements Signed-off-by: mohammad alqudah * check goal pose validity before setting collision free distance map Signed-off-by: mohammad alqudah * declare variables as const where necessary Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- planning/autoware_freespace_planner/README.md | 7 +- .../config/freespace_planner.param.yaml | 12 +- .../freespace_planner_node.cpp | 21 +- .../abstract_algorithm.hpp | 70 +++- .../astar_search.hpp | 65 ++-- .../kinematic_bicycle_model.hpp | 73 ++++ .../scripts/bind/astar_search_pybind.cpp | 24 +- .../scripts/example/example.py | 10 +- .../src/abstract_algorithm.cpp | 61 ++-- .../src/astar_search.cpp | 335 ++++++++++-------- .../src/rrtstar.cpp | 13 +- .../test_freespace_planning_algorithms.cpp | 32 +- .../config/goal_planner.param.yaml | 5 +- .../src/manager.cpp | 30 +- .../config/start_planner.param.yaml | 5 +- .../src/manager.cpp | 30 +- 16 files changed, 474 insertions(+), 319 deletions(-) create mode 100644 planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index 6bf13a81c0413..e0fd41a70cd36 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -59,14 +59,15 @@ None | Parameter | Type | Description | | ------------------------- | ------ | -------------------------------------------------- | | `time_limit` | double | time limit of planning | -| `minimum_turning_radius` | double | minimum turning radius of robot | -| `maximum_turning_radius` | double | maximum turning radius of robot | +| `maximum_turning_ratio` | double | max ratio of actual turning range to use | +| `turning_steps` | double | number of turning steps within turning range | | `theta_size` | double | the number of angle's discretization | | `lateral_goal_range` | double | goal range of lateral position | | `longitudinal_goal_range` | double | goal range of longitudinal position | | `angle_goal_range` | double | goal range of angle | | `curve_weight` | double | additional cost factor for curve actions | | `reverse_weight` | double | additional cost factor for reverse actions | +| `direction_change_weight` | double | additional cost factor for switching direction | | `obstacle_threshold` | double | threshold for regarding a certain grid as obstacle | #### A\* search parameters @@ -75,7 +76,9 @@ None | --------------------------- | ------ | ------------------------------------------------------- | | `only_behind_solutions` | bool | whether restricting the solutions to be behind the goal | | `use_back` | bool | whether using backward trajectory | +| `expansion_distance` | double | length of expansion for node transitions | | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost | +| `smoothness_weight` | double | cost factor for change in curvature | #### RRT\* search parameters diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml index 8692aa293cbf7..ee631f8639153 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -15,16 +15,16 @@ # -- Configurations common to the all planners -- # base configs time_limit: 30000.0 - minimum_turning_radius: 9.0 - maximum_turning_radius: 9.0 - turning_radius_size: 1 + max_turning_ratio: 0.5 # ratio of actual turning limit of vehicle + turning_steps: 1 # search configs theta_size: 144 angle_goal_range: 6.0 - curve_weight: 1.2 - reverse_weight: 2.0 lateral_goal_range: 0.5 longitudinal_goal_range: 2.0 + curve_weight: 0.5 + reverse_weight: 1.0 + direction_change_weight: 1.5 # costmap configs obstacle_threshold: 100 @@ -32,7 +32,9 @@ astar: only_behind_solutions: false use_back: true + expansion_distance: 0.5 distance_heuristic_weight: 1.0 + smoothness_weight: 0.5 # -- RRT* search Configurations -- rrtstar: diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 55068263f5d5b..d79a06ae52ead 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -245,6 +245,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_shape_.length = vehicle_info.vehicle_length_m; vehicle_shape_.width = vehicle_info.vehicle_width_m; + vehicle_shape_.base_length = vehicle_info.wheel_base_m; + vehicle_shape_.max_steering = vehicle_info.max_steer_angle_rad; vehicle_shape_.base2back = vehicle_info.rear_overhang_m; } @@ -296,18 +298,16 @@ PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() // search configs p.time_limit = declare_parameter("time_limit"); - p.minimum_turning_radius = declare_parameter("minimum_turning_radius"); - p.maximum_turning_radius = declare_parameter("maximum_turning_radius"); - p.turning_radius_size = declare_parameter("turning_radius_size"); - p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); - p.turning_radius_size = std::max(p.turning_radius_size, 1); p.theta_size = declare_parameter("theta_size"); p.angle_goal_range = declare_parameter("angle_goal_range"); p.curve_weight = declare_parameter("curve_weight"); p.reverse_weight = declare_parameter("reverse_weight"); + p.direction_change_weight = declare_parameter("direction_change_weight"); p.lateral_goal_range = declare_parameter("lateral_goal_range"); p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range"); + p.max_turning_ratio = declare_parameter("max_turning_ratio"); + p.turning_steps = declare_parameter("turning_steps"); // costmap configs p.obstacle_threshold = declare_parameter("obstacle_threshold"); @@ -517,7 +517,13 @@ void FreespacePlannerNode::planTrajectory() // execute planning const rclcpp::Time start = get_clock()->now(); - const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + std::string error_msg; + bool result = false; + try { + result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + } catch (const std::exception & e) { + error_msg = e.what(); + } const rclcpp::Time end = get_clock()->now(); RCLCPP_DEBUG(get_logger(), "Freespace planning: %f [s]", (end - start).seconds()); @@ -530,9 +536,8 @@ void FreespacePlannerNode::planTrajectory() prev_target_index_ = 0; target_index_ = getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_); - } else { - RCLCPP_INFO(get_logger(), "Can't find goal..."); + RCLCPP_INFO(get_logger(), "Can't find goal: %s", error_msg.c_str()); reset(); } } 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 bca08f0d11c58..4f0a59f44a5d6 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 @@ -23,6 +23,7 @@ #include +#include #include namespace autoware::freespace_planning_algorithms @@ -57,14 +58,21 @@ geometry_msgs::msg::Pose local2global( struct VehicleShape { - double length; // X [m] - double width; // Y [m] + double length; // X [m] + double width; // Y [m] + double base_length; + double max_steering; double base2back; // base_link to rear [m] VehicleShape() = default; - VehicleShape(double length, double width, double base2back) - : length(length), width(width), base2back(base2back) + VehicleShape( + double length, double width, double base_length, double max_steering, double base2back) + : length(length), + width(width), + base_length(base_length), + max_steering(max_steering), + base2back(base2back) { } @@ -72,6 +80,8 @@ struct VehicleShape const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : length(vehicle_info.vehicle_length_m + margin), width(vehicle_info.vehicle_width_m + margin), + base_length(vehicle_info.wheel_base_m), + max_steering(vehicle_info.max_steer_angle_rad), base2back(vehicle_info.rear_overhang_m + margin / 2.0) { } @@ -82,18 +92,16 @@ struct PlannerCommonParam // base configs double time_limit; // planning time limit [msec] - // robot configs - double minimum_turning_radius; // [m] - double maximum_turning_radius; // [m] - int turning_radius_size; // discretized turning radius table size [-] - // search configs int theta_size; // discretized angle table size [-] double curve_weight; // curve moving cost [-] double reverse_weight; // backward moving cost [-] + double direction_change_weight; // direction change cost [-] double lateral_goal_range; // reaching threshold, lateral error [m] double longitudinal_goal_range; // reaching threshold, longitudinal error [m] double angle_goal_range; // reaching threshold, angle error [deg] + double max_turning_ratio; // factor of max turning range to use [-] + int turning_steps; // number of turning steps [-] // costmap configs int obstacle_threshold; // obstacle threshold on grid [-] @@ -120,6 +128,8 @@ class AbstractPlanningAlgorithm const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape) : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape) { + planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1); + collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio; is_collision_table_initialized = false; } @@ -128,6 +138,8 @@ class AbstractPlanningAlgorithm const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) { + planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1); + collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio; } virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); @@ -142,8 +154,11 @@ class AbstractPlanningAlgorithm void computeCollisionIndexes( int theta_index, std::vector & indexes, std::vector & vertex_indexes_2d) const; + bool detectBoundaryExit(const IndexXYT & base_index) const; bool detectCollision(const IndexXYT & base_index) const; - inline bool isOutOfRange(const IndexXYT & index) const + + template + inline bool isOutOfRange(const IndexType & index) const { if (index.x < 0 || static_cast(costmap_.info.width) <= index.x) { return true; @@ -153,16 +168,41 @@ class AbstractPlanningAlgorithm } return false; } - inline bool isObs(const IndexXYT & index) const + + template + inline bool isWithinMargin(const IndexType & index) const + { + if ( + index.x < nb_of_margin_cells_ || + static_cast(costmap_.info.width) - index.x < nb_of_margin_cells_) { + return false; + } + if ( + index.y < nb_of_margin_cells_ || + static_cast(costmap_.info.height) - index.y < nb_of_margin_cells_) { + return false; + } + return true; + } + + template + inline bool isObs(const IndexType & index) const { // NOTE: Accessing by .at() instead makes 1.2 times slower here. // Also, boundary check is already done in isOutOfRange before calling this function. // So, basically .at() is not necessary. - return is_obstacle_table_[index.y][index.x]; + return is_obstacle_table_[indexToId(index)]; + } + + // compute single dimensional grid cell index from 2 dimensional index + template + inline int indexToId(const IndexType & index) const + { + return index.y * costmap_.info.width + index.x; } PlannerCommonParam planner_common_param_; - const VehicleShape collision_vehicle_shape_; + VehicleShape collision_vehicle_shape_; // costmap as occupancy grid nav_msgs::msg::OccupancyGrid costmap_; @@ -174,7 +214,7 @@ class AbstractPlanningAlgorithm std::vector> vertex_indexes_table_; // is_obstacle's table - std::vector> is_obstacle_table_; + std::vector is_obstacle_table_; // pose in costmap frame geometry_msgs::msg::Pose start_pose_; @@ -185,6 +225,8 @@ class AbstractPlanningAlgorithm // result path PlannerWaypoints waypoints_; + + int nb_of_margin_cells_; }; } // namespace autoware::freespace_planning_algorithms 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 a746ceea6f838..3f6c3935b8a42 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 @@ -34,6 +34,8 @@ namespace autoware::freespace_planning_algorithms { +using geometry_msgs::msg::Pose; + enum class NodeStatus : uint8_t { None, Open, Closed }; struct AstarParam @@ -41,9 +43,11 @@ struct AstarParam // base configs bool only_behind_solutions; // solutions should be behind the goal bool use_back; // backward search + double expansion_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] + double smoothness_weight; }; struct AstarNode @@ -52,20 +56,30 @@ struct AstarNode double x; // x double y; // y double theta; // theta - double gc = 0; // actual cost - double hc = 0; // heuristic cost + double gc = 0; // actual motion cost + double fc = 0; // total node cost + double dir_distance = 0; // distance traveled from last direction change + int steering_index; // steering index bool is_back; // true if the current direction of the vehicle is back AstarNode * parent = nullptr; // parent node - double cost() const { return gc + hc; } + inline void set( + const Pose & pose, const double move_cost, const double total_cost, const double steer_ind, + const bool backward) + { + x = pose.position.x; + y = pose.position.y; + theta = tf2::getYaw(pose.orientation); + gc = move_cost; + fc = total_cost; + steering_index = steer_ind; + is_back = backward; + } }; struct NodeComparison { - bool operator()(const AstarNode * lhs, const AstarNode * rhs) - { - return lhs->cost() > rhs->cost(); - } + bool operator()(const AstarNode * lhs, const AstarNode * rhs) { return lhs->fc > rhs->fc; } }; struct NodeUpdate @@ -74,7 +88,7 @@ struct NodeUpdate double shift_y; double shift_theta; double distance; - bool is_curve; + int steering_index; bool is_back; NodeUpdate rotated(const double theta) const @@ -120,43 +134,45 @@ class AstarSearch : public AbstractPlanningAlgorithm AstarParam{ node.declare_parameter("astar.only_behind_solutions"), node.declare_parameter("astar.use_back"), - node.declare_parameter("astar.distance_heuristic_weight")}) + node.declare_parameter("astar.expansion_distance"), + node.declare_parameter("astar.distance_heuristic_weight"), + node.declare_parameter("astar.smoothness_weight")}) { } - void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; - bool makePlan( - const geometry_msgs::msg::Pose & start_pose, - const geometry_msgs::msg::Pose & goal_pose) override; + bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; const PlannerWaypoints & getWaypoints() const { return waypoints_; } inline int getKey(const IndexXYT & index) { - return (index.theta + (index.y * x_scale_ + index.x) * y_scale_); + return indexToId(index) * planner_common_param_.theta_size + index.theta; } private: + void setTransitionTable(); + void setCollisionFreeDistanceMap(); bool search(); - void clearNodes(); + void expandNodes(AstarNode & current_node); + void resetData(); void setPath(const AstarNode & goal); bool setStartNode(); bool setGoalNode(); - double estimateCost(const geometry_msgs::msg::Pose & pose) const; + double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; - geometry_msgs::msg::Pose node2pose(const AstarNode & node) const; + Pose node2pose(const AstarNode & node) const; - AstarNode * getNodeRef(const IndexXYT & index) - { - return &(graph_.emplace(getKey(index), AstarNode()).first->second); - } + double getSteeringCost(const int steering_index) const; + double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const; + double getDirectionChangeCost(const double dir_distance) const; // Algorithm specific param AstarParam astar_param_; // hybrid astar variables TransitionTable transition_table_; - std::unordered_map graph_; + std::vector graph_; + std::vector col_free_distance_map_; std::priority_queue, NodeComparison> openlist_; @@ -166,8 +182,9 @@ class AstarSearch : public AbstractPlanningAlgorithm // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; - int x_scale_; - int y_scale_; + double steering_resolution_; + double heading_resolution_; + double avg_turning_radius_; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp new file mode 100644 index 0000000000000..0a59646f34d52 --- /dev/null +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// 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. + +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ + +#include +#include + +#include + +#include + +namespace autoware::freespace_planning_algorithms +{ +namespace kinematic_bicycle_model +{ + +static constexpr double eps = 0.001; + +inline double getTurningRadius(const double base_length, const double steering_angle) +{ + return base_length / tan(steering_angle); +} + +inline geometry_msgs::msg::Pose getPoseShift( + const double yaw, const double base_length, const double steering_angle, const double distance) +{ + geometry_msgs::msg::Pose pose; + if (abs(steering_angle) < eps) { + pose.position.x = distance * cos(yaw); + pose.position.y = distance * sin(yaw); + return pose; + } + const double R = getTurningRadius(base_length, steering_angle); + const double beta = distance / R; + pose.position.x = R * sin(yaw + beta) - R * sin(yaw); + pose.position.y = R * cos(yaw) - R * cos(yaw + beta); + pose.orientation = autoware::universe_utils::createQuaternionFromYaw(beta); + return pose; +} + +inline geometry_msgs::msg::Pose getPose( + const geometry_msgs::msg::Pose & current_pose, const double base_length, + const double steering_angle, const double distance) +{ + const double current_yaw = tf2::getYaw(current_pose.orientation); + const auto shift = getPoseShift(current_yaw, base_length, steering_angle, distance); + auto pose = current_pose; + pose.position.x += shift.position.x; + pose.position.y += shift.position.y; + if (abs(steering_angle) > eps) { + pose.orientation = autoware::universe_utils::createQuaternionFromYaw( + current_yaw + tf2::getYaw(shift.orientation)); + } + return pose; +} + +} // namespace kinematic_bicycle_model +} // namespace autoware::freespace_planning_algorithms + +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ 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 8932fb277abe8..70de72f2bfffc 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 @@ -114,28 +114,30 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) .def_readwrite( "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions) .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back) + .def_readwrite( + "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) .def_readwrite( "distance_heuristic_weight", - &freespace_planning_algorithms::AstarParam::distance_heuristic_weight); + &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) + .def_readwrite( + "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight); auto pyPlannerCommonParam = py::class_( p, "PlannerCommonParam", py::dynamic_attr()) .def(py::init<>()) .def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit) + .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size) .def_readwrite( - "minimum_turning_radius", - &freespace_planning_algorithms::PlannerCommonParam::minimum_turning_radius) - .def_readwrite( - "maximum_turning_radius", - &freespace_planning_algorithms::PlannerCommonParam::maximum_turning_radius) + "max_turning_ratio", &freespace_planning_algorithms::PlannerCommonParam::max_turning_ratio) .def_readwrite( - "turning_radius_size", - &freespace_planning_algorithms::PlannerCommonParam::turning_radius_size) - .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size) + "turning_steps", &freespace_planning_algorithms::PlannerCommonParam::turning_steps) .def_readwrite( "curve_weight", &freespace_planning_algorithms::PlannerCommonParam::curve_weight) .def_readwrite( "reverse_weight", &freespace_planning_algorithms::PlannerCommonParam::reverse_weight) + .def_readwrite( + "direction_change_weight", + &freespace_planning_algorithms::PlannerCommonParam::direction_change_weight) .def_readwrite( "lateral_goal_range", &freespace_planning_algorithms::PlannerCommonParam::lateral_goal_range) @@ -150,9 +152,11 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) auto pyVehicleShape = py::class_(p, "VehicleShape", py::dynamic_attr()) .def(py::init<>()) - .def(py::init()) + .def(py::init()) .def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length) .def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width) + .def_readwrite("base_length", &freespace_planning_algorithms::VehicleShape::base_length) + .def_readwrite("max_steering", &freespace_planning_algorithms::VehicleShape::max_steering) .def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back); py::class_( diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index c0cdf8c2ec6ba..93e3acbc345b6 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -28,14 +28,14 @@ planner_param = fp.PlannerCommonParam() # base configs planner_param.time_limit = 30000.0 -planner_param.minimum_turning_radius = 9.0 -planner_param.maximum_turning_radius = 9.0 -planner_param.turning_radius_size = 1 +planner_param.max_turning_ratio = 0.5 +planner_param.turning_steps = 1 # search configs planner_param.theta_size = 144 planner_param.angle_goal_range = 6.0 planner_param.curve_weight = 1.2 -planner_param.reverse_weight = 2.0 +planner_param.reverse_weight = 1.0 +planner_param.direction_change_weight = 2.0 planner_param.lateral_goal_range = 0.5 planner_param.longitudinal_goal_range = 2.0 # costmap configs @@ -46,7 +46,9 @@ astar_param = fp.AstarParam() astar_param.only_behind_solutions = False astar_param.use_back = True +astar_param.expansion_distance = 0.4 astar_param.distance_heuristic_weight = 1.0 +astar_param.smoothness_weight = 1.0 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param) diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index c434635ff1401..7ec594d389de3 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,6 +14,8 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" + #include #include @@ -35,16 +37,16 @@ geometry_msgs::msg::Pose transformPose( int discretizeAngle(const double theta, const int theta_size) { - const double one_angle_range = 2.0 * M_PI / theta_size; - return static_cast(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size; + const double angle_resolution = 2.0 * M_PI / theta_size; + return static_cast(std::round(normalizeRadian(theta, 0.0) / angle_resolution)) % theta_size; } IndexXYT pose2index( const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local, const int theta_size) { - const int index_x = pose_local.position.x / costmap.info.resolution; - const int index_y = pose_local.position.y / costmap.info.resolution; + const int index_x = std::round(pose_local.position.x / costmap.info.resolution); + const int index_y = std::round(pose_local.position.y / costmap.info.resolution); const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size); return {index_x, index_y, index_theta}; } @@ -57,8 +59,8 @@ geometry_msgs::msg::Pose index2pose( pose_local.position.x = index.x * costmap.info.resolution; pose_local.position.y = index.y * costmap.info.resolution; - const double one_angle_range = 2.0 * M_PI / theta_size; - const double yaw = index.theta * one_angle_range; + const double angle_resolution = 2.0 * M_PI / theta_size; + const double yaw = index.theta * angle_resolution; pose_local.orientation = createQuaternionFromYaw(yaw); return pose_local; @@ -105,20 +107,15 @@ double PlannerWaypoints::compute_length() const void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & costmap) { costmap_ = costmap; - const auto height = costmap_.info.height; - const auto width = costmap_.info.width; + const uint32_t nb_of_cells = costmap_.data.size(); // Initialize status - std::vector> is_obstacle_table; - is_obstacle_table.resize(height); - for (uint32_t i = 0; i < height; i++) { - is_obstacle_table.at(i).resize(width); - for (uint32_t j = 0; j < width; j++) { - const int cost = costmap_.data[i * width + j]; - - if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) { - is_obstacle_table[i][j] = true; - } + std::vector is_obstacle_table; + is_obstacle_table.resize(nb_of_cells); + for (uint32_t i = 0; i < nb_of_cells; ++i) { + const int cost = costmap_.data[i]; + if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) { + is_obstacle_table[i] = true; } } is_obstacle_table_ = is_obstacle_table; @@ -133,6 +130,10 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost } is_collision_table_initialized = true; } + + const double base2front = collision_vehicle_shape_.length - collision_vehicle_shape_.base2back; + nb_of_margin_cells_ = std::ceil( + std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution); } void AbstractPlanningAlgorithm::computeCollisionIndexes( @@ -201,16 +202,12 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( addIndex2d(back, left, vertex_indexes_2d); } -bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const +bool AbstractPlanningAlgorithm::detectBoundaryExit(const IndexXYT & base_index) const { - if (coll_indexes_table_.empty()) { - std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; - return false; - } - + if (isWithinMargin(base_index)) return false; const auto & vertex_indexes_2d = vertex_indexes_table_[base_index.theta]; for (const auto & vertex_index_2d : vertex_indexes_2d) { - IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0}; + IndexXY vertex_index{vertex_index_2d.x, vertex_index_2d.y}; // must slide to current base position vertex_index.x += base_index.x; vertex_index.y += base_index.y; @@ -218,11 +215,21 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con return true; } } + return false; +} + +bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const +{ + if (coll_indexes_table_.empty()) { + std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; + return false; + } + + if (detectBoundaryExit(base_index)) return true; const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { - int idx_theta = 0; // whatever. Yaw is nothing to do with collision detection between grids. - IndexXYT coll_index{coll_index_2d.x, coll_index_2d.y, idx_theta}; + IndexXY coll_index{coll_index_2d.x, coll_index_2d.y}; // must slide to current base position coll_index.x += base_index.x; coll_index.y += base_index.y; diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 1a75c8277ebfd..4ead3cb5a7423 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -14,11 +14,15 @@ #include "autoware/freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" + #include #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -29,8 +33,7 @@ namespace autoware::freespace_planning_algorithms { -double calcReedsSheppDistance( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, double radius) +double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius) { const auto rs_space = ReedsSheppStateSpace(radius); const ReedsSheppStateSpace::StateXYT pose0{ @@ -45,8 +48,7 @@ void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw) *orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); } -geometry_msgs::msg::Pose calcRelativePose( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & pose) +Pose calcRelativePose(const Pose & base_pose, const Pose & pose) { tf2::Transform tf_transform; tf2::convert(base_pose, tf_transform); @@ -62,55 +64,6 @@ geometry_msgs::msg::Pose calcRelativePose( return transformed.pose; } -AstarSearch::TransitionTable createTransitionTable( - const double minimum_turning_radius, const double maximum_turning_radius, - const int turning_radius_size, const double theta_size, const bool use_back) -{ - // Vehicle moving for each angle - AstarSearch::TransitionTable transition_table; - transition_table.resize(theta_size); - - const double dtheta = 2.0 * M_PI / theta_size; - - // Minimum moving distance with one state update - // arc = r * theta - const auto & R_min = minimum_turning_radius; - const auto & R_max = maximum_turning_radius; - const double step_min = R_min * dtheta; - const double dR = (R_max - R_min) / std::max(turning_radius_size - 1, 1); - - // NodeUpdate actions - std::vector forward_node_candidates; - const NodeUpdate forward_straight{step_min, 0.0, 0.0, step_min, false, false}; - forward_node_candidates.push_back(forward_straight); - for (int i = 0; i < turning_radius_size; ++i) { - double R = R_min + i * dR; - double step = R * dtheta; - const double shift_x = R * sin(dtheta); - const double shift_y = R * (1 - cos(dtheta)); - const NodeUpdate forward_left{shift_x, shift_y, dtheta, step, true, false}; - const NodeUpdate forward_right = forward_left.flipped(); - forward_node_candidates.push_back(forward_left); - forward_node_candidates.push_back(forward_right); - } - - for (int i = 0; i < theta_size; i++) { - const double theta = dtheta * i; - - for (const auto & nu : forward_node_candidates) { - transition_table[i].push_back(nu.rotated(theta)); - } - - if (use_back) { - for (const auto & nu : forward_node_candidates) { - transition_table[i].push_back(nu.reversed().rotated(theta)); - } - } - } - - return transition_table; -} - AstarSearch::AstarSearch( const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, const AstarParam & astar_param) @@ -119,66 +72,134 @@ AstarSearch::AstarSearch( goal_node_(nullptr), use_reeds_shepp_(true) { - transition_table_ = createTransitionTable( - planner_common_param_.minimum_turning_radius, planner_common_param_.maximum_turning_radius, - planner_common_param_.turning_radius_size, planner_common_param_.theta_size, - astar_param_.use_back); + steering_resolution_ = + collision_vehicle_shape_.max_steering / planner_common_param_.turning_steps; + heading_resolution_ = 2.0 * M_PI / planner_common_param_.theta_size; - y_scale_ = planner_common_param.theta_size; + const double avg_steering = + steering_resolution_ + (collision_vehicle_shape_.max_steering - steering_resolution_) / 2.0; + avg_turning_radius_ = + kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering); + + setTransitionTable(); } -void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) +void AstarSearch::setTransitionTable() { - AbstractPlanningAlgorithm::setMap(costmap); + const double distance = astar_param_.expansion_distance; + transition_table_.resize(planner_common_param_.theta_size); + + std::vector forward_transitions; + int steering_ind = -1 * planner_common_param_.turning_steps; + for (; steering_ind <= planner_common_param_.turning_steps; ++steering_ind) { + const double steering = static_cast(steering_ind) * steering_resolution_; + Pose shift_pose = kinematic_bicycle_model::getPoseShift( + 0.0, collision_vehicle_shape_.base_length, steering, distance); + forward_transitions.push_back( + {shift_pose.position.x, shift_pose.position.y, tf2::getYaw(shift_pose.orientation), distance, + steering_ind, false}); + } + + for (int i = 0; i < planner_common_param_.theta_size; ++i) { + const double theta = static_cast(i) * heading_resolution_; + for (const auto & transition : forward_transitions) { + transition_table_[i].push_back(transition.rotated(theta)); + } - x_scale_ = costmap_.info.height; + if (astar_param_.use_back) { + for (const auto & transition : forward_transitions) { + transition_table_[i].push_back(transition.reversed().rotated(theta)); + } + } + } } -bool AstarSearch::makePlan( - const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) +bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) { + resetData(); + start_pose_ = global2local(costmap_, start_pose); goal_pose_ = global2local(costmap_, goal_pose); - clearNodes(); - graph_.reserve(100000); + if (!setGoalNode()) { + throw std::logic_error("Invalid goal pose"); + } + + setCollisionFreeDistanceMap(); if (!setStartNode()) { - return false; + throw std::logic_error("Invalid start pose"); } - if (!setGoalNode()) { - return false; + if (!search()) { + throw std::logic_error("HA* failed to find path to goal"); } - return search(); + return true; } -void AstarSearch::clearNodes() +void AstarSearch::resetData() { // clearing openlist is necessary because otherwise remaining elements of openlist // point to deleted node. openlist_ = std::priority_queue, NodeComparison>(); + const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height; + const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size; + graph_ = std::vector(total_astar_node_count); + col_free_distance_map_ = + std::vector(nb_of_grid_nodes, std::numeric_limits::max()); +} - graph_ = std::unordered_map(); +void AstarSearch::setCollisionFreeDistanceMap() +{ + using Entry = std::pair; + struct CompareEntry + { + bool operator()(const Entry & a, const Entry & b) { return a.second > b.second; } + }; + std::priority_queue, CompareEntry> heap; + std::vector closed(col_free_distance_map_.size(), false); + auto goal_index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size); + col_free_distance_map_[indexToId(goal_index)] = 0.0; + heap.push({IndexXY{goal_index.x, goal_index.y}, 0.0}); + + Entry current; + std::array offsets = {1, 0, -1}; + while (!heap.empty()) { + current = heap.top(); + heap.pop(); + const int id = indexToId(current.first); + if (closed[id]) continue; + closed[id] = true; + + const auto & index = current.first; + for (const auto & offset_x : offsets) { + const int x = index.x + offset_x; + for (const auto & offset_y : offsets) { + const int y = index.y + offset_y; + const IndexXY n_index{x, y}; + const double offset = std::abs(offset_x) + std::abs(offset_y); + if (isOutOfRange(n_index) || isObs(n_index) || offset < 1) continue; + const int n_id = indexToId(n_index); + const double dist = current.second + (sqrt(offset) * costmap_.info.resolution); + if (closed[n_id] || col_free_distance_map_[n_id] < dist) continue; + col_free_distance_map_[n_id] = dist; + heap.push({n_index, dist}); + } + } + } } bool AstarSearch::setStartNode() { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); - if (detectCollision(index)) { - return false; - } + if (detectCollision(index)) return false; // Set start node - AstarNode * start_node = getNodeRef(index); - start_node->x = start_pose_.position.x; - start_node->y = start_pose_.position.y; - start_node->theta = 2.0 * M_PI / planner_common_param_.theta_size * index.theta; - start_node->gc = 0; - start_node->hc = estimateCost(start_pose_); - start_node->is_back = false; + AstarNode * start_node = &graph_[getKey(index)]; + start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); + start_node->dir_distance = 0.0; start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -191,29 +212,18 @@ bool AstarSearch::setStartNode() bool AstarSearch::setGoalNode() { const auto index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size); - - if (detectCollision(index)) { - return false; - } - - return true; + return !detectCollision(index); } -double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) const +double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const { - double total_cost = 0.0; + double total_cost = col_free_distance_map_[indexToId(index)]; // Temporarily, until reeds_shepp gets stable. if (use_reeds_shepp_) { - const double radius = (planner_common_param_.minimum_turning_radius + - planner_common_param_.maximum_turning_radius) * - 0.5; - total_cost += - calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight; - } else { - total_cost += autoware::universe_utils::calcDistance2d(pose, goal_pose_) * - astar_param_.distance_heuristic_weight; + total_cost = + std::max(total_cost, calcReedsSheppDistance(pose, goal_pose_, avg_turning_radius_)); } - return total_cost; + return astar_param_.distance_heuristic_weight * total_cost; } bool AstarSearch::search() @@ -232,6 +242,7 @@ bool AstarSearch::search() // Expand minimum cost node AstarNode * current_node = openlist_.top(); openlist_.pop(); + if (current_node->status == NodeStatus::Closed) continue; current_node->status = NodeStatus::Closed; if (isGoal(*current_node)) { @@ -240,45 +251,81 @@ bool AstarSearch::search() return true; } - // Transit - const auto index_theta = discretizeAngle(current_node->theta, planner_common_param_.theta_size); - for (const auto & transition : transition_table_[index_theta]) { - const bool is_turning_point = transition.is_back != current_node->is_back; + expandNodes(*current_node); + } + + // Failed to find path + return false; +} + +void AstarSearch::expandNodes(AstarNode & current_node) +{ + const auto index_theta = discretizeAngle(current_node.theta, planner_common_param_.theta_size); + for (const auto & transition : transition_table_[index_theta]) { + // skip transition back to parent + if ( + current_node.parent != nullptr && transition.is_back != current_node.is_back && + transition.steering_index == current_node.steering_index) { + continue; + } + + // Calculate index of the next state + Pose next_pose; + next_pose.position.x = current_node.x + transition.shift_x; + next_pose.position.y = current_node.y + transition.shift_y; + setYaw(&next_pose.orientation, current_node.theta + transition.shift_theta); + const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size); - const double move_cost = is_turning_point - ? planner_common_param_.reverse_weight * transition.distance - : transition.distance; + if (isOutOfRange(next_index) || isObs(next_index)) continue; - // Calculate index of the next state - geometry_msgs::msg::Pose next_pose; - next_pose.position.x = current_node->x + transition.shift_x; - next_pose.position.y = current_node->y + transition.shift_y; - setYaw(&next_pose.orientation, current_node->theta + transition.shift_theta); - const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size); + AstarNode * next_node = &graph_[getKey(next_index)]; + if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue; - if (detectCollision(next_index)) { - continue; - } + const bool is_direction_switch = + (current_node.parent != nullptr) && (transition.is_back != current_node.is_back); - // Compare cost - AstarNode * next_node = getNodeRef(next_index); - if (next_node->status == NodeStatus::None) { - next_node->status = NodeStatus::Open; - next_node->x = next_pose.position.x; - next_node->y = next_pose.position.y; - next_node->theta = tf2::getYaw(next_pose.orientation); - next_node->gc = current_node->gc + move_cost; - next_node->hc = estimateCost(next_pose); - next_node->is_back = transition.is_back; - next_node->parent = current_node; - openlist_.push(next_node); - continue; - } + double total_weight = 1.0; + total_weight += getSteeringCost(transition.steering_index); + if (transition.is_back) { + total_weight *= (1.0 + planner_common_param_.reverse_weight); + } + + double move_cost = current_node.gc + (total_weight * transition.distance); + move_cost += getSteeringChangeCost(transition.steering_index, current_node.steering_index); + if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance); + + double total_cost = move_cost + estimateCost(next_pose, next_index); + // Compare cost + if (next_node->status == NodeStatus::None || next_node->fc > total_cost) { + next_node->status = NodeStatus::Open; + next_node->set( + next_pose, move_cost, total_cost, transition.steering_index, transition.is_back); + next_node->dir_distance = + transition.distance + (is_direction_switch ? 0.0 : current_node.dir_distance); + next_node->parent = ¤t_node; + openlist_.push(next_node); + continue; } } +} - // Failed to find path - return false; +double AstarSearch::getSteeringCost(const int steering_index) const +{ + return planner_common_param_.curve_weight * + (abs(steering_index) / planner_common_param_.turning_steps); +} + +double AstarSearch::getSteeringChangeCost( + const int steering_index, const int prev_steering_index) const +{ + double steering_index_diff = abs(steering_index - prev_steering_index); + return astar_param_.smoothness_weight * steering_index_diff / + (2.0 * planner_common_param_.turning_steps); +} + +double AstarSearch::getDirectionChangeCost(const double dir_distance) const +{ + return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance))); } void AstarSearch::setPath(const AstarNode & goal_node) @@ -293,30 +340,12 @@ void AstarSearch::setPath(const AstarNode & goal_node) // From the goal node to the start node const AstarNode * node = &goal_node; - // push exact goal pose first - { - geometry_msgs::msg::PoseStamped pose; - pose.header = header; - pose.pose = local2global(costmap_, goal_pose_); - - PlannerWaypoint pw; - pw.pose = pose; - pw.is_back = node->is_back; - waypoints_.waypoints.push_back(pw); - } - + geometry_msgs::msg::PoseStamped pose; + pose.header = header; // push astar nodes poses while (node != nullptr) { - geometry_msgs::msg::PoseStamped pose; - pose.header = header; pose.pose = local2global(costmap_, node2pose(*node)); - - // PlannerWaypoint - PlannerWaypoint pw; - pw.pose = pose; - pw.is_back = node->is_back; - waypoints_.waypoints.push_back(pw); - + waypoints_.waypoints.push_back({pose, node->is_back}); // To the next node node = node->parent; } @@ -359,9 +388,9 @@ bool AstarSearch::isGoal(const AstarNode & node) const return true; } -geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const +Pose AstarSearch::node2pose(const AstarNode & node) const { - geometry_msgs::msg::Pose pose_local; + Pose pose_local; pose_local.position.x = node.x; pose_local.position.y = node.y; diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 6b0018c517d4a..0ef31959e65fa 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -14,6 +14,8 @@ #include "autoware/freespace_planning_algorithms/rrtstar.hpp" +#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" + namespace autoware::freespace_planning_algorithms { rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg) @@ -29,6 +31,7 @@ RRTStar::RRTStar( planner_common_param, VehicleShape( original_vehicle_shape.length + 2 * rrtstar_param.margin, original_vehicle_shape.width + 2 * rrtstar_param.margin, + original_vehicle_shape.base_length, original_vehicle_shape.max_steering, original_vehicle_shape.base2back + rrtstar_param.margin)), rrtstar_param_(rrtstar_param), original_vehicle_shape_(original_vehicle_shape) @@ -36,9 +39,6 @@ RRTStar::RRTStar( if (rrtstar_param_.margin <= 0) { throw std::invalid_argument("rrt's collision margin must be greater than 0"); } - if (planner_common_param_.maximum_turning_radius != planner_common_param.minimum_turning_radius) { - throw std::invalid_argument("Currently supports only single radius in rrtstar."); - } } bool RRTStar::makePlan( @@ -50,8 +50,8 @@ bool RRTStar::makePlan( goal_pose_ = global2local(costmap_, goal_pose); const auto is_obstacle_free = [&](const rrtstar_core::Pose & pose) { - const int index_x = pose.x / costmap_.info.resolution; - const int index_y = pose.y / costmap_.info.resolution; + const int index_x = std::round(pose.x / costmap_.info.resolution); + const int index_y = std::round(pose.y / costmap_.info.resolution); const int index_theta = discretizeAngle(pose.yaw, planner_common_param_.theta_size); return !detectCollision(IndexXYT{index_x, index_y, index_theta}); }; @@ -60,7 +60,8 @@ bool RRTStar::makePlan( const rrtstar_core::Pose hi{ costmap_.info.resolution * costmap_.info.width, costmap_.info.resolution * costmap_.info.height, M_PI}; - const double radius = planner_common_param_.minimum_turning_radius; + const double radius = kinematic_bicycle_model::getTurningRadius( + collision_vehicle_shape_.base_length, collision_vehicle_shape_.max_steering); const auto cspace = rrtstar_core::CSpace(lo, hi, radius, is_obstacle_free); const auto x_start = poseMsgToPose(start_pose_); const auto x_goal = poseMsgToPose(goal_pose_); 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 79898ed57327b..31c551e1906db 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 @@ -37,7 +37,10 @@ namespace fpa = autoware::freespace_planning_algorithms; const double length_lexus = 5.5; const double width_lexus = 2.75; -const fpa::VehicleShape vehicle_shape = fpa::VehicleShape(length_lexus, width_lexus, 1.5); +const double base_length_lexus = 3.0; +const double max_steering_lexus = 0.7; +const fpa::VehicleShape vehicle_shape = + fpa::VehicleShape(length_lexus, width_lexus, base_length_lexus, max_steering_lexus, 1.5); const double pi = 3.1415926; const std::array start_pose{5.5, 4., pi * 0.5}; const std::array goal_pose1{8.0, 26.3, pi * 1.5}; // easiest @@ -170,15 +173,14 @@ fpa::PlannerCommonParam get_default_planner_params() { // set problem configuration const double time_limit = 10 * 1000.0; - const double minimum_turning_radius = 9.0; - const double maximum_turning_radius = 9.0; - const int turning_radius_size = 1; + const double max_turning_ratio = 0.5; + const int turning_steps = 1; const int theta_size = 144; - // Setting weight to 1.0 to fairly compare all algorithms - const double curve_weight = 1.0; + const double curve_weight = 0.5; const double reverse_weight = 1.0; + const double direction_change_weight = 1.5; const double lateral_goal_range = 0.5; const double longitudinal_goal_range = 2.0; @@ -187,15 +189,15 @@ fpa::PlannerCommonParam get_default_planner_params() return fpa::PlannerCommonParam{ time_limit, - minimum_turning_radius, - maximum_turning_radius, - turning_radius_size, theta_size, curve_weight, reverse_weight, + direction_change_weight, lateral_goal_range, longitudinal_goal_range, angle_goal_range, + max_turning_ratio, + turning_steps, obstacle_threshold}; } @@ -203,16 +205,18 @@ std::unique_ptr configure_astar(bool use_multi) { auto planner_common_param = get_default_planner_params(); if (use_multi) { - planner_common_param.maximum_turning_radius = 14.0; - planner_common_param.turning_radius_size = 3; + planner_common_param.turning_steps = 3; } // configure astar param const bool only_behind_solutions = false; const bool use_back = true; - const double distance_heuristic_weight = 1.0; - const auto astar_param = - fpa::AstarParam{only_behind_solutions, use_back, distance_heuristic_weight}; + const double expansion_distance = 0.4; + const double distance_heuristic_weight = 2.0; + const double smoothness_weight = 0.5; + const auto astar_param = fpa::AstarParam{ + only_behind_solutions, use_back, expansion_distance, distance_heuristic_weight, + smoothness_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 e6f84e7eece0c..61908b524c253 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 @@ -90,9 +90,8 @@ velocity: 1.0 vehicle_shape_margin: 1.0 time_limit: 3000.0 - minimum_turning_radius: 5.0 - maximum_turning_radius: 5.0 - turning_radius_size: 1 + max_turning_ratio: 0.7 + turning_steps: 1 # search configs search_configs: theta_size: 144 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 b86bbbca7d98b..8a4f3ab202a67 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 @@ -201,17 +201,10 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); p.freespace_parking_common_parameters.time_limit = node->declare_parameter(ns + "time_limit"); - p.freespace_parking_common_parameters.minimum_turning_radius = - node->declare_parameter(ns + "minimum_turning_radius"); - p.freespace_parking_common_parameters.maximum_turning_radius = - node->declare_parameter(ns + "maximum_turning_radius"); - p.freespace_parking_common_parameters.turning_radius_size = - node->declare_parameter(ns + "turning_radius_size"); - p.freespace_parking_common_parameters.maximum_turning_radius = std::max( - p.freespace_parking_common_parameters.maximum_turning_radius, - p.freespace_parking_common_parameters.minimum_turning_radius); - p.freespace_parking_common_parameters.turning_radius_size = - std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); + p.freespace_parking_common_parameters.max_turning_ratio = + node->declare_parameter(ns + "max_turning_ratio"); + p.freespace_parking_common_parameters.turning_steps = + node->declare_parameter(ns + "turning_steps"); } // freespace parking search config @@ -590,19 +583,10 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "time_limit", p->freespace_parking_common_parameters.time_limit); updateParam( - parameters, ns + "minimum_turning_radius", - p->freespace_parking_common_parameters.minimum_turning_radius); - updateParam( - parameters, ns + "maximum_turning_radius", - p->freespace_parking_common_parameters.maximum_turning_radius); + parameters, ns + "max_turning_ratio", + p->freespace_parking_common_parameters.max_turning_ratio); updateParam( - parameters, ns + "turning_radius_size", - p->freespace_parking_common_parameters.turning_radius_size); - p->freespace_parking_common_parameters.maximum_turning_radius = std::max( - p->freespace_parking_common_parameters.maximum_turning_radius, - p->freespace_parking_common_parameters.minimum_turning_radius); - p->freespace_parking_common_parameters.turning_radius_size = - std::max(p->freespace_parking_common_parameters.turning_radius_size, 1); + parameters, ns + "turning_steps", p->freespace_parking_common_parameters.turning_steps); } // freespace parking search config 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 cb34f9962c479..851e96f7a265c 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 @@ -62,9 +62,8 @@ velocity: 1.0 vehicle_shape_margin: 1.0 time_limit: 3000.0 - minimum_turning_radius: 5.0 - maximum_turning_radius: 5.0 - turning_radius_size: 1 + max_turning_ratio: 0.7 + turning_steps: 1 # search configs search_configs: theta_size: 144 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 facb39c821764..23817ee081501 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 @@ -137,17 +137,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); p.freespace_planner_common_parameters.time_limit = node->declare_parameter(ns + "time_limit"); - p.freespace_planner_common_parameters.minimum_turning_radius = - node->declare_parameter(ns + "minimum_turning_radius"); - p.freespace_planner_common_parameters.maximum_turning_radius = - node->declare_parameter(ns + "maximum_turning_radius"); - p.freespace_planner_common_parameters.turning_radius_size = - node->declare_parameter(ns + "turning_radius_size"); - p.freespace_planner_common_parameters.maximum_turning_radius = std::max( - p.freespace_planner_common_parameters.maximum_turning_radius, - p.freespace_planner_common_parameters.minimum_turning_radius); - p.freespace_planner_common_parameters.turning_radius_size = - std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + p.freespace_planner_common_parameters.max_turning_ratio = + node->declare_parameter(ns + "max_turning_ratio"); + p.freespace_planner_common_parameters.turning_steps = + node->declare_parameter(ns + "turning_steps"); } // freespace planner search config { @@ -473,19 +466,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); updateParam( - parameters, ns + "minimum_turning_radius", - p->freespace_planner_common_parameters.minimum_turning_radius); - updateParam( - parameters, ns + "maximum_turning_radius", - p->freespace_planner_common_parameters.maximum_turning_radius); + parameters, ns + "max_turning_ratio", + p->freespace_planner_common_parameters.max_turning_ratio); updateParam( - parameters, ns + "turning_radius_size", - p->freespace_planner_common_parameters.turning_radius_size); - p->freespace_planner_common_parameters.maximum_turning_radius = std::max( - p->freespace_planner_common_parameters.maximum_turning_radius, - p->freespace_planner_common_parameters.minimum_turning_radius); - p->freespace_planner_common_parameters.turning_radius_size = - std::max(p->freespace_planner_common_parameters.turning_radius_size, 1); + parameters, ns + "turning_steps", p->freespace_planner_common_parameters.turning_steps); } { const std::string ns = "start_planner.freespace_planner.search_configs.";