From 2a050d78f3a119abeaafe4c8c4cb770bc9610758 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 14 Aug 2024 13:14:46 +0900 Subject: [PATCH] feat(freespace_planning_algorithms): use distance to nearest obstacle to improve path planning (#8089) * 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 * implement edt map to store distance to nearest obstacle for each grid cell Signed-off-by: mohammad alqudah * use obstacle edt in collision check Signed-off-by: mohammad alqudah * add cost for distance to obstacle 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 * add missing include 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 * implement adaptive expansion distance Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * interpolate nodes with large expansion distance 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 * use obstacle edt when computing collision free distance 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 * refactor detect collision function Signed-off-by: mohammad alqudah * add missing variable initialization Signed-off-by: mohammad alqudah * remove declared but undefined function Signed-off-by: mohammad alqudah * remove unnecessary checks Signed-off-by: mohammad alqudah * minor fix Signed-off-by: mohammad alqudah * refactor computeEDTMap function Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * set min and max expansion distance after setting costmap Signed-off-by: mohammad alqudah * refactor detectCollision function Signed-off-by: mohammad alqudah * remove unused function Signed-off-by: mohammad alqudah * change default parameter values Signed-off-by: mohammad alqudah * fix computeEDTMap function Signed-off-by: mohammad alqudah * rename parameter Signed-off-by: mohammad alqudah * use linear function for obstacle distance cost 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 * compare front and back lengths when setting min and max dimension Signed-off-by: mohammad alqudah * add docstring and citation for computeEDTMap function Signed-off-by: mohammad alqudah * suppress spell check Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- planning/autoware_freespace_planner/README.md | 2 + .../config/freespace_planner.param.yaml | 4 +- .../freespace_planner_node.cpp | 1 + .../abstract_algorithm.hpp | 36 +++++ .../astar_search.hpp | 73 ++++------ .../kinematic_bicycle_model.hpp | 37 ++--- .../scripts/bind/astar_search_pybind.cpp | 8 +- .../scripts/example/example.py | 2 + .../src/abstract_algorithm.cpp | 86 +++++++++++- .../src/astar_search.cpp | 126 ++++++++++-------- .../src/rrtstar.cpp | 4 +- .../test_freespace_planning_algorithms.cpp | 6 +- 12 files changed, 250 insertions(+), 135 deletions(-) diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index e0fd41a70cd36..91c880924857d 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -76,9 +76,11 @@ None | --------------------------- | ------ | ------------------------------------------------------- | | `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 | | `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 | +| `obstacle_distance_weight` | double | cost factor for distance to obstacle | #### 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 ee631f8639153..b4a1951514604 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -32,9 +32,11 @@ astar: only_behind_solutions: false use_back: true + adapt_expansion_distance: true expansion_distance: 0.5 - distance_heuristic_weight: 1.0 + distance_heuristic_weight: 1.5 smoothness_weight: 0.5 + obstacle_distance_weight: 1.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 d79a06ae52ead..b077f79f88071 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 @@ -573,6 +573,7 @@ void FreespacePlannerNode::initializePlanningAlgorithm() extended_vehicle_shape.length += margin; extended_vehicle_shape.width += margin; extended_vehicle_shape.base2back += margin / 2; + extended_vehicle_shape.setMinMaxDimension(); const auto planner_common_param = getPlannerCommonParam(); 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 4f0a59f44a5d6..cbf835df3b6c6 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 @@ -24,6 +24,7 @@ #include #include +#include #include namespace autoware::freespace_planning_algorithms @@ -63,6 +64,8 @@ struct VehicleShape double base_length; double max_steering; double base2back; // base_link to rear [m] + double min_dimension; + double max_dimension; VehicleShape() = default; @@ -74,6 +77,7 @@ struct VehicleShape max_steering(max_steering), base2back(base2back) { + setMinMaxDimension(); } explicit VehicleShape( @@ -84,6 +88,19 @@ struct VehicleShape max_steering(vehicle_info.max_steer_angle_rad), base2back(vehicle_info.rear_overhang_m + margin / 2.0) { + setMinMaxDimension(); + } + + void setMinMaxDimension() + { + const auto base2front = length - base2back; + if (base2back <= base2front) { + min_dimension = std::min(0.5 * width, base2back); + max_dimension = std::hypot(base2front, 0.5 * width); + } else { + min_dimension = std::min(0.5 * width, base2front); + max_dimension = std::hypot(base2back, 0.5 * width); + } } }; @@ -156,6 +173,16 @@ class AbstractPlanningAlgorithm std::vector & vertex_indexes_2d) const; bool detectBoundaryExit(const IndexXYT & base_index) const; bool detectCollision(const IndexXYT & base_index) const; + bool detectCollision(const geometry_msgs::msg::Pose & base_pose) const; + + // cspell: ignore Toriwaki + /// @brief Computes the euclidean distance to the nearest obstacle for each grid cell. + /// @cite T., Saito, and J., Toriwaki "New algorithms for euclidean distance transformation of an + /// n-dimensional digitized picture with applications," Pattern Recognition 27, 1994 + /// https://doi.org/10.1016/0031-3203(94)90133-3 + /// @details first, distance values are computed along each row. Then, the computed values are + /// used to to compute the minimum distance along each column. + void computeEDTMap(); template inline bool isOutOfRange(const IndexType & index) const @@ -194,6 +221,12 @@ class AbstractPlanningAlgorithm return is_obstacle_table_[indexToId(index)]; } + template + inline double getObstacleEDT(const IndexType & index) const + { + return edt_map_[indexToId(index)]; + } + // compute single dimensional grid cell index from 2 dimensional index template inline int indexToId(const IndexType & index) const @@ -216,6 +249,9 @@ class AbstractPlanningAlgorithm // is_obstacle's table std::vector is_obstacle_table_; + // Euclidean distance transform map (distance to nearest obstacle cell) + std::vector edt_map_; + // pose in costmap frame geometry_msgs::msg::Pose start_pose_; geometry_msgs::msg::Pose goal_pose_; 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 f7434d912d5e0..cbdbb9d4c3fb3 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 @@ -43,11 +43,13 @@ struct AstarParam // base configs bool only_behind_solutions; // solutions should be behind the goal bool use_back; // backward search + bool adapt_expansion_distance; double expansion_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] double smoothness_weight; + double obstacle_distance_weight; }; struct AstarNode @@ -56,9 +58,11 @@ struct AstarNode double x; // x double y; // y double theta; // theta - double gc = 0; // actual motion cost - double fc = 0; // total node cost - double dir_distance = 0; // distance traveled from last direction change + double gc = 0.0; // actual motion cost + double fc = 0.0; // total node cost + double dir_distance = 0.0; // distance traveled from last direction change + double dist_to_goal = 0.0; // euclidean distance to goal pose + double dist_to_obs = 0.0; // euclidean distance to nearest obstacle int steering_index; // steering index bool is_back; // true if the current direction of the vehicle is back AstarNode * parent = nullptr; // parent node @@ -82,46 +86,9 @@ struct NodeComparison bool operator()(const AstarNode * lhs, const AstarNode * rhs) const { return lhs->fc > rhs->fc; } }; -struct NodeUpdate -{ - double shift_x; - double shift_y; - double shift_theta; - double distance; - int steering_index; - bool is_back; - - NodeUpdate rotated(const double theta) const - { - NodeUpdate result = *this; - result.shift_x = std::cos(theta) * this->shift_x - std::sin(theta) * this->shift_y; - result.shift_y = std::sin(theta) * this->shift_x + std::cos(theta) * this->shift_y; - return result; - } - - NodeUpdate flipped() const - { - NodeUpdate result = *this; - result.shift_y = -result.shift_y; - result.shift_theta = -result.shift_theta; - return result; - } - - NodeUpdate reversed() const - { - NodeUpdate result = *this; - result.shift_x = -result.shift_x; - result.shift_theta = -result.shift_theta; - result.is_back = !result.is_back; - return result; - } -}; - class AstarSearch : public AbstractPlanningAlgorithm { public: - using TransitionTable = std::vector>; - AstarSearch( const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, const AstarParam & astar_param); @@ -134,12 +101,15 @@ class AstarSearch : public AbstractPlanningAlgorithm AstarParam{ node.declare_parameter("astar.only_behind_solutions"), node.declare_parameter("astar.use_back"), + node.declare_parameter("astar.adapt_expansion_distance"), node.declare_parameter("astar.expansion_distance"), node.declare_parameter("astar.distance_heuristic_weight"), - node.declare_parameter("astar.smoothness_weight")}) + node.declare_parameter("astar.smoothness_weight"), + node.declare_parameter("astar.obstacle_distance_weight")}) { } + void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; const PlannerWaypoints & getWaypoints() const { return waypoints_; } @@ -150,27 +120,26 @@ class AstarSearch : public AbstractPlanningAlgorithm } private: - void setTransitionTable(); void setCollisionFreeDistanceMap(); bool search(); - void expandNodes(AstarNode & current_node); + void expandNodes(AstarNode & current_node, const bool is_back = false); void resetData(); void setPath(const AstarNode & goal); bool setStartNode(); - bool setGoalNode(); double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; Pose node2pose(const AstarNode & node) const; + double getExpansionDistance(const AstarNode & current_node) const; 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; + double getObsDistanceCost(const double obs_distance) const; // Algorithm specific param AstarParam astar_param_; // hybrid astar variables - TransitionTable transition_table_; std::vector graph_; std::vector col_free_distance_map_; @@ -185,6 +154,20 @@ class AstarSearch : public AbstractPlanningAlgorithm double steering_resolution_; double heading_resolution_; double avg_turning_radius_; + double min_expansion_dist_; + double max_expansion_dist_; + + // 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 + // parameters. + + // expansion distance factors + static constexpr double base_length_max_expansion_factor_ = 0.5; + static constexpr double dist_to_goal_expansion_factor_ = 0.15; + static constexpr double dist_to_obs_expansion_factor_ = 0.3; + + // cost free obstacle distance + static constexpr double cost_free_obs_dist = 5.0; }; } // 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 index 0a59646f34d52..b9b6fe8c79af0 100644 --- 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 @@ -31,39 +31,26 @@ 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; + return base_length / std::tan(steering_angle); } 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)); + const double yaw = tf2::getYaw(current_pose.orientation); + if (std::abs(steering_angle) < eps) { + pose.position.x += distance * std::cos(yaw); + pose.position.y += distance * std::sin(yaw); + return pose; } + + const double R = getTurningRadius(base_length, steering_angle); + const double beta = distance / R; + pose.position.x += (R * std::sin(yaw + beta) - R * std::sin(yaw)); + pose.position.y += (R * std::cos(yaw) - R * std::cos(yaw + beta)); + pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw + beta); return pose; } 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 70de72f2bfffc..40b16f71d5fe7 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,13 +114,19 @@ 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( + "adapt_expansion_distance", + &freespace_planning_algorithms::AstarParam::adapt_expansion_distance) .def_readwrite( "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) .def_readwrite( "distance_heuristic_weight", &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) .def_readwrite( - "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight); + "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight) + .def_readwrite( + "obstacle_distance_weight", + &freespace_planning_algorithms::AstarParam::obstacle_distance_weight); auto pyPlannerCommonParam = py::class_( p, "PlannerCommonParam", py::dynamic_attr()) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index 93e3acbc345b6..28c505fbaae02 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -46,9 +46,11 @@ astar_param = fp.AstarParam() astar_param.only_behind_solutions = False astar_param.use_back = True +astar_param.adapt_expansion_distance = True astar_param.expansion_distance = 0.4 astar_param.distance_heuristic_weight = 1.0 astar_param.smoothness_weight = 1.0 +astar_param.obstacle_distance_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 7ec594d389de3..380eafb745062 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -120,6 +120,8 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost } is_obstacle_table_ = is_obstacle_table; + computeEDTMap(); + // construct collision indexes table if (is_collision_table_initialized == false) { for (int i = 0; i < planner_common_param_.theta_size; i++) { @@ -136,6 +138,73 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution); } +void AbstractPlanningAlgorithm::computeEDTMap() +{ + const int height = costmap_.info.height; + const int width = costmap_.info.width; + const double resolution_m = costmap_.info.resolution; + const double diagonal_m = std::hypot(resolution_m * height, resolution_m * width); + std::vector edt_map; + edt_map.reserve(costmap_.data.size()); + + std::vector temporary_storage(width); + // scan rows + for (int i = 0; i < height; ++i) { + double distance = resolution_m; + bool found_obstacle = false; + // forward scan + for (int j = 0; j < width; ++j) { + if (isObs(IndexXY{j, i})) { + temporary_storage[j] = 0.0; + distance = resolution_m; + found_obstacle = true; + } else if (found_obstacle) { + temporary_storage[j] = distance; + distance += resolution_m; + } else { + temporary_storage[j] = diagonal_m; + } + } + + distance = resolution_m; + found_obstacle = false; + // backward scan + for (int j = width - 1; j >= 0; --j) { + if (isObs(IndexXY{j, i})) { + distance = resolution_m; + found_obstacle = true; + } else if (found_obstacle && temporary_storage[j] > distance) { + temporary_storage[j] = distance; + distance += resolution_m; + } + } + edt_map.insert(edt_map.end(), temporary_storage.begin(), temporary_storage.end()); + } + + temporary_storage.clear(); + temporary_storage.resize(height); + // scan columns; + for (int j = 0; j < width; ++j) { + for (int i = 0; i < height; ++i) { + int id = indexToId(IndexXY{j, i}); + double min_value = edt_map[id] * edt_map[id]; + for (int k = 0; k < height; ++k) { + id = indexToId(IndexXY{j, k}); + double dist = resolution_m * std::abs(static_cast(i - k)); + double value = edt_map[id] * edt_map[id] + dist * dist; + if (value < min_value) { + min_value = value; + } + } + temporary_storage[i] = sqrt(min_value); + } + for (int i = 0; i < height; ++i) { + edt_map[indexToId(IndexXY{j, i})] = temporary_storage[i]; + } + } + edt_map_ = edt_map; +} + void AbstractPlanningAlgorithm::computeCollisionIndexes( int theta_index, std::vector & indexes_2d, std::vector & vertex_indexes_2d) const @@ -218,6 +287,12 @@ bool AbstractPlanningAlgorithm::detectBoundaryExit(const IndexXYT & base_index) return false; } +bool AbstractPlanningAlgorithm::detectCollision(const geometry_msgs::msg::Pose & base_pose) const +{ + const auto base_index = pose2index(costmap_, base_pose, planner_common_param_.theta_size); + return detectCollision(base_index); +} + bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const { if (coll_indexes_table_.empty()) { @@ -227,6 +302,13 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con if (detectBoundaryExit(base_index)) return true; + double obstacle_edt = getObstacleEDT(base_index); + + // if nearest obstacle is further than largest dimension, no collision is guaranteed + // if nearest obstacle is closer than smallest dimension, collision is guaranteed + if (obstacle_edt > collision_vehicle_shape_.max_dimension) return false; + if (obstacle_edt < collision_vehicle_shape_.min_dimension) return true; + const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { IndexXY coll_index{coll_index_2d.x, coll_index_2d.y}; @@ -247,9 +329,7 @@ bool AbstractPlanningAlgorithm::hasObstacleOnTrajectory( { for (const auto & pose : trajectory.poses) { const auto pose_local = global2local(costmap_, pose); - const auto index = pose2index(costmap_, pose_local, planner_common_param_.theta_size); - - if (detectCollision(index)) { + if (detectCollision(pose_local)) { return true; } } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 9233ad212bfa5..fe87011273c1b 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -33,6 +33,8 @@ namespace autoware::freespace_planning_algorithms { +using autoware::universe_utils::calcDistance2d; + double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius) { const auto rs_space = ReedsSheppStateSpace(radius); @@ -81,37 +83,18 @@ AstarSearch::AstarSearch( avg_turning_radius_ = kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering); - setTransitionTable(); + min_expansion_dist_ = astar_param_.expansion_distance; + max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; } -void AstarSearch::setTransitionTable() +void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & 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)); - } + AbstractPlanningAlgorithm::setMap(costmap); - if (astar_param_.use_back) { - for (const auto & transition : forward_transitions) { - transition_table_[i].push_back(transition.reversed().rotated(theta)); - } - } - } + // ensure minimum expansion distance is larger then grid cell diagonal length + min_expansion_dist_ = std::max(astar_param_.expansion_distance, 1.5 * costmap_.info.resolution); + max_expansion_dist_ = std::max( + collision_vehicle_shape_.base_length * base_length_max_expansion_factor_, min_expansion_dist_); } bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) @@ -121,7 +104,7 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) start_pose_ = global2local(costmap_, start_pose); goal_pose_ = global2local(costmap_, goal_pose); - if (!setGoalNode()) { + if (detectCollision(goal_pose_)) { throw std::logic_error("Invalid goal pose"); } @@ -180,6 +163,7 @@ void AstarSearch::setCollisionFreeDistanceMap() 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; + if (getObstacleEDT(n_index) < 0.5 * collision_vehicle_shape_.width) 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; @@ -200,6 +184,8 @@ bool AstarSearch::setStartNode() 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->dist_to_goal = calcDistance2d(start_pose_, goal_pose_); + start_node->dist_to_obs = getObstacleEDT(index); start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -209,12 +195,6 @@ bool AstarSearch::setStartNode() return true; } -bool AstarSearch::setGoalNode() -{ - const auto index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size); - return !detectCollision(index); -} - double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const { double total_cost = col_free_distance_map_[indexToId(index)]; @@ -252,28 +232,29 @@ bool AstarSearch::search() } expandNodes(*current_node); + if (astar_param_.use_back) expandNodes(*current_node, true); } // Failed to find path return false; } -void AstarSearch::expandNodes(AstarNode & current_node) +void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) { - 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 + const auto current_pose = node2pose(current_node); + double distance = getExpansionDistance(current_node) * (is_back ? -1.0 : 1.0); + int steering_index = -1 * planner_common_param_.turning_steps; + for (; steering_index <= planner_common_param_.turning_steps; ++steering_index) { + // skip expansion back to parent if ( - current_node.parent != nullptr && transition.is_back != current_node.is_back && - transition.steering_index == current_node.steering_index) { + current_node.parent != nullptr && is_back != current_node.is_back && + 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 double steering = static_cast(steering_index) * steering_resolution_; + const auto next_pose = kinematic_bicycle_model::getPose( + current_pose, collision_vehicle_shape_.base_length, steering, distance); const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size); if (isOutOfRange(next_index) || isObs(next_index)) continue; @@ -281,27 +262,28 @@ void AstarSearch::expandNodes(AstarNode & current_node) AstarNode * next_node = &graph_[getKey(next_index)]; if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue; + const double distance_to_obs = getObstacleEDT(next_index); const bool is_direction_switch = - (current_node.parent != nullptr) && (transition.is_back != current_node.is_back); + (current_node.parent != nullptr) && (is_back != current_node.is_back); double total_weight = 1.0; - total_weight += getSteeringCost(transition.steering_index); - if (transition.is_back) { - total_weight *= (1.0 + planner_common_param_.reverse_weight); - } + total_weight += getSteeringCost(steering_index); + if (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); + double move_cost = current_node.gc + (total_weight * std::abs(distance)); + move_cost += getSteeringChangeCost(steering_index, current_node.steering_index); + move_cost += getObsDistanceCost(distance_to_obs); 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->set(next_pose, move_cost, total_cost, steering_index, is_back); next_node->dir_distance = - transition.distance + (is_direction_switch ? 0.0 : current_node.dir_distance); + std::abs(distance) + (is_direction_switch ? 0.0 : current_node.dir_distance); + next_node->dist_to_goal = calcDistance2d(next_pose, goal_pose_); + next_node->dist_to_obs = distance_to_obs; next_node->parent = ¤t_node; openlist_.push(next_node); continue; @@ -309,6 +291,17 @@ void AstarSearch::expandNodes(AstarNode & current_node) } } +double AstarSearch::getExpansionDistance(const AstarNode & current_node) const +{ + if (!astar_param_.adapt_expansion_distance || max_expansion_dist_ <= min_expansion_dist_) { + return min_expansion_dist_; + } + double exp_dist = std::min( + current_node.dist_to_goal * dist_to_goal_expansion_factor_, + current_node.dist_to_obs * dist_to_obs_expansion_factor_); + return std::clamp(exp_dist, min_expansion_dist_, max_expansion_dist_); +} + double AstarSearch::getSteeringCost(const int steering_index) const { return planner_common_param_.curve_weight * @@ -328,6 +321,12 @@ double AstarSearch::getDirectionChangeCost(const double dir_distance) const return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance))); } +double AstarSearch::getObsDistanceCost(const double obs_distance) const +{ + return astar_param_.obstacle_distance_weight * + std::max(1.0 - (obs_distance / cost_free_obs_dist), 0.0); +} + void AstarSearch::setPath(const AstarNode & goal_node) { std_msgs::msg::Header header; @@ -342,10 +341,27 @@ void AstarSearch::setPath(const AstarNode & goal_node) geometry_msgs::msg::PoseStamped pose; pose.header = header; + + const auto interpolate = [this, &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 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}); + } + }; + // push astar nodes poses while (node != nullptr) { pose.pose = local2global(costmap_, node2pose(*node)); waypoints_.waypoints.push_back({pose, node->is_back}); + interpolate(*node); // To the next node node = node->parent; } diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 0ef31959e65fa..20a59048e59d1 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -113,9 +113,7 @@ bool RRTStar::hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & traj { for (const auto & pose : trajectory.poses) { const auto pose_local = global2local(costmap_, pose); - const auto base_index = pose2index(costmap_, pose_local, planner_common_param_.theta_size); - - if (detectCollision(base_index)) { + if (detectCollision(pose_local)) { return true; } } 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 31c551e1906db..7284a8dcffc44 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 @@ -211,12 +211,14 @@ std::unique_ptr configure_astar(bool use_multi) // configure astar param const bool only_behind_solutions = false; const bool use_back = true; + const bool adapt_expansion_distance = true; const double expansion_distance = 0.4; const double distance_heuristic_weight = 2.0; const double smoothness_weight = 0.5; + const double obstacle_distance_weight = 1.7; const auto astar_param = fpa::AstarParam{ - only_behind_solutions, use_back, expansion_distance, distance_heuristic_weight, - smoothness_weight}; + 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;