From 9c04530194447d1963c6aedb22b368069c26020a Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 23 Aug 2024 11:25:52 +0900 Subject: [PATCH] feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner (#8092) * 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 * implement backward search option 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 * remove unnecessary code Signed-off-by: mohammad alqudah * make A-star search work with multiple goal candidates as input Signed-off-by: mohammad alqudah * fix is_back flag logic 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 * fix interpolation for backward search 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 * use flag instead of enum Signed-off-by: mohammad alqudah * add missing variable initialization Signed-off-by: mohammad alqudah * remove declared but undefined function Signed-off-by: mohammad alqudah * refactor makePlan() function Signed-off-by: mohammad alqudah * remove bool return statement for void 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 * enable both forward and backward search options for multiple goal candidates 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 * add missing last waypoint 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 * add public access function to get distance to nearest obstacle Signed-off-by: mohammad alqudah * compare node index with goal index in isGoal check Signed-off-by: mohammad alqudah * append shifted goal pose to waypoints for more accurate arrival 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 * initialize vectors using assign function 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 * fix shifted goal pose for backward search Signed-off-by: mohammad alqudah * transform pose to local frame in getDistanceToObstacle funcion Signed-off-by: mohammad alqudah * add cost for lateral distance near goal Signed-off-by: mohammad alqudah * compute distance to obstacle from ego frame instead of base Signed-off-by: mohammad alqudah * update freespace planner parameter schema Signed-off-by: mohammad alqudah * update freespace planner parameter schema Signed-off-by: mohammad alqudah * refactor setPath function Signed-off-by: mohammad alqudah * fix function setPath Signed-off-by: mohammad alqudah * declare bool var as constant Signed-off-by: mohammad alqudah * remove unnecessary includes Signed-off-by: mohammad alqudah * minor refactor 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 +- .../schema/freespace_planner.schema.json | 10 + .../freespace_planner_node.cpp | 3 +- .../abstract_algorithm.hpp | 38 +++- .../astar_search.hpp | 28 ++- .../freespace_planning_algorithms/rrtstar.hpp | 3 + .../scripts/bind/astar_search_pybind.cpp | 7 +- .../scripts/example/example.py | 2 + .../src/abstract_algorithm.cpp | 40 ++-- .../src/astar_search.cpp | 204 ++++++++++++++---- .../src/rrtstar.cpp | 7 + .../test_freespace_planning_algorithms.cpp | 14 +- 13 files changed, 290 insertions(+), 72 deletions(-) diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index db48154ffa214..56e096944e739 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -82,9 +82,11 @@ None | `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 | +| `near_goal_distance` | double | near goal distance threshold | | `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 | +| `goal_lat_distance_weight` | double | cost factor for lateral distance from goal | #### 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 6ea99b2c0c061..b34ffe698aa36 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -22,7 +22,7 @@ theta_size: 144 angle_goal_range: 6.0 lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 + longitudinal_goal_range: 1.0 curve_weight: 0.5 reverse_weight: 1.0 direction_change_weight: 1.5 @@ -36,9 +36,11 @@ use_back: true adapt_expansion_distance: true expansion_distance: 0.5 + near_goal_distance: 4.0 distance_heuristic_weight: 1.5 smoothness_weight: 0.5 obstacle_distance_weight: 1.5 + goal_lat_distance_weight: 5.0 # -- RRT* search Configurations -- rrtstar: diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json index 4494878849897..e42995718974a 100644 --- a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json +++ b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json @@ -146,6 +146,11 @@ "default": 0.5, "description": "Distance for expanding nodes in A* search." }, + "near_goal_distance": { + "type": "number", + "default": 4.0, + "description": "Distance threshold to consider near goal." + }, "distance_heuristic_weight": { "type": "number", "default": 1.0, @@ -160,6 +165,11 @@ "type": "number", "default": 0.5, "description": "Weight for distance to obstacle in A* search." + }, + "goal_lat_distance_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for lateral distance from original goal." } }, "required": [ 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 9bff2cfddd2c6..8e8a962ed1071 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 @@ -377,8 +377,7 @@ void FreespacePlannerNode::updateTargetIndex() } else { // Switch to next partial trajectory prev_target_index_ = target_index_; - target_index_ = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); + target_index_ = new_target_index; } } } 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 911144f3796e5..8a4d6bf2e42b5 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 @@ -16,6 +16,7 @@ #define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #include +#include #include #include @@ -29,6 +30,10 @@ namespace autoware::freespace_planning_algorithms { +using autoware::universe_utils::normalizeRadian; + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); int discretizeAngle(const double theta, const int theta_size); struct IndexXYT @@ -36,6 +41,11 @@ struct IndexXYT int x; int y; int theta; + + bool operator==(const IndexXYT & index) const + { + return (x == index.x && y == index.y && theta == index.theta); + } }; struct IndexXY @@ -138,6 +148,12 @@ struct PlannerWaypoints double compute_length() const; }; +struct EDTData +{ + double distance; + double angle; +}; + class AbstractPlanningAlgorithm { public: @@ -162,6 +178,9 @@ class AbstractPlanningAlgorithm virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); virtual bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; + virtual bool makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) = 0; virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } double getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const; @@ -223,7 +242,7 @@ class AbstractPlanningAlgorithm } template - inline double getObstacleEDT(const IndexType & index) const + inline EDTData getObstacleEDT(const IndexType & index) const { return edt_map_[indexToId(index)]; } @@ -235,6 +254,19 @@ class AbstractPlanningAlgorithm return index.y * costmap_.info.width + index.x; } + inline double getVehicleBaseToFrameDistance(const double angle) const + { + const double normalized_angle = std::abs(normalizeRadian(angle)); + const double w = 0.5 * collision_vehicle_shape_.width; + const double l_b = collision_vehicle_shape_.base2back; + const double l_f = collision_vehicle_shape_.length - l_b; + + if (normalized_angle < atan(w / l_f)) return l_f / cos(normalized_angle); + if (normalized_angle < M_PI_2) return w / sin(normalized_angle); + if (normalized_angle < M_PI_2 + atan(l_b / w)) return w / cos(normalized_angle - M_PI_2); + return l_b / cos(M_PI - normalized_angle); + } + PlannerCommonParam planner_common_param_; VehicleShape collision_vehicle_shape_; @@ -250,8 +282,8 @@ class AbstractPlanningAlgorithm // is_obstacle's table std::vector is_obstacle_table_; - // Euclidean distance transform map (distance to nearest obstacle cell) - std::vector edt_map_; + // Euclidean distance transform map (distance & angle info to nearest obstacle cell) + std::vector edt_map_; // pose in costmap frame geometry_msgs::msg::Pose start_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 8ac7c60e7ca4f..65ef53d820cba 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 @@ -23,6 +23,8 @@ #include #include +#include + #include #include #include @@ -46,11 +48,13 @@ struct AstarParam bool use_back; // backward search bool adapt_expansion_distance; double expansion_distance; + double near_goal_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] double smoothness_weight; double obstacle_distance_weight; + double goal_lat_distance_weight; }; struct AstarNode @@ -105,15 +109,19 @@ class AstarSearch : public AbstractPlanningAlgorithm node.declare_parameter("astar.use_back"), node.declare_parameter("astar.adapt_expansion_distance"), node.declare_parameter("astar.expansion_distance"), + node.declare_parameter("astar.near_goal_distance"), node.declare_parameter("astar.distance_heuristic_weight"), node.declare_parameter("astar.smoothness_weight"), - node.declare_parameter("astar.obstacle_distance_weight")}) + node.declare_parameter("astar.obstacle_distance_weight"), + node.declare_parameter("astar.goal_lat_distance_weight")}) { } void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; + bool makePlan(const Pose & start_pose, const std::vector & goal_candidates) override; + const PlannerWaypoints & getWaypoints() const { return waypoints_; } inline int getKey(const IndexXYT & index) @@ -127,16 +135,18 @@ class AstarSearch : public AbstractPlanningAlgorithm void expandNodes(AstarNode & current_node, const bool is_back = false); void resetData(); void setPath(const AstarNode & goal); - void setStartNode(); + void setStartNode(const double cost_offset = 0.0); double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; + void setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) 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; + double getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const; + double getLatDistanceCost(const Pose & pose) const; // Algorithm specific param AstarParam astar_param_; @@ -150,6 +160,11 @@ class AstarSearch : public AbstractPlanningAlgorithm // goal node, which may helpful in testing and debugging AstarNode * goal_node_; + mutable boost::optional shifted_goal_pose_; + + // alternate goals for when multiple goal candidates are given + std::vector alternate_goals_; + // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; @@ -158,7 +173,9 @@ class AstarSearch : public AbstractPlanningAlgorithm double avg_turning_radius_; double min_expansion_dist_; double max_expansion_dist_; + double near_goal_dist_; bool is_backward_search_; + bool is_multiple_goals_; // 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 @@ -169,8 +186,11 @@ class AstarSearch : public AbstractPlanningAlgorithm static constexpr double dist_to_goal_expansion_factor_ = 0.15; static constexpr double dist_to_obs_expansion_factor_ = 0.3; + // initial cost offset for multi goal backward search + static constexpr double multi_goal_backward_cost_offset = 5.0; + // cost free obstacle distance - static constexpr double cost_free_obs_dist = 5.0; + static constexpr double cost_free_obs_dist = 1.0; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp index 17e7f1f1ebf56..b30692ce8da2d 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp @@ -58,6 +58,9 @@ class RRTStar : public AbstractPlanningAlgorithm bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) override; + bool makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) override; bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const override; private: 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 1c04fd65447b2..c35081bd97f84 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 @@ -120,6 +120,8 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) &freespace_planning_algorithms::AstarParam::adapt_expansion_distance) .def_readwrite( "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) + .def_readwrite( + "near_goal_distance", &freespace_planning_algorithms::AstarParam::near_goal_distance) .def_readwrite( "distance_heuristic_weight", &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) @@ -127,7 +129,10 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight) .def_readwrite( "obstacle_distance_weight", - &freespace_planning_algorithms::AstarParam::obstacle_distance_weight); + &freespace_planning_algorithms::AstarParam::obstacle_distance_weight) + .def_readwrite( + "goal_lat_distance_weight", + &freespace_planning_algorithms::AstarParam::goal_lat_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 b4af9ed87a3b9..9f18dc799c9d0 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -49,9 +49,11 @@ astar_param.use_back = True astar_param.adapt_expansion_distance = True astar_param.expansion_distance = 0.4 +astar_param.near_goal_distance = 3.0 astar_param.distance_heuristic_weight = 1.0 astar_param.smoothness_weight = 1.0 astar_param.obstacle_distance_weight = 1.0 +astar_param.goal_lat_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 97fd93e31d5cd..b968e70bd2c01 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -19,12 +19,12 @@ #include #include +#include #include namespace autoware::freespace_planning_algorithms { using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -145,19 +145,19 @@ double AbstractPlanningAlgorithm::getDistanceToObstacle(const geometry_msgs::msg if (indexToId(index) >= static_cast(edt_map_.size())) { return std::numeric_limits::max(); } - return getObstacleEDT(index); + return getObstacleEDT(index).distance; } void AbstractPlanningAlgorithm::computeEDTMap() { + edt_map_.clear(); 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; + std::vector> edt_map; edt_map.reserve(costmap_.data.size()); - std::vector temporary_storage(width); + std::vector> temporary_storage(width); // scan rows for (int i = 0; i < height; ++i) { double distance = resolution_m; @@ -165,14 +165,16 @@ void AbstractPlanningAlgorithm::computeEDTMap() // forward scan for (int j = 0; j < width; ++j) { if (isObs(IndexXY{j, i})) { - temporary_storage[j] = 0.0; + temporary_storage[j].first = 0.0; + temporary_storage[j].second.x = 0.0; distance = resolution_m; found_obstacle = true; } else if (found_obstacle) { - temporary_storage[j] = distance; + temporary_storage[j].first = distance; + temporary_storage[j].second.x = -distance; distance += resolution_m; } else { - temporary_storage[j] = diagonal_m; + temporary_storage[j].first = std::numeric_limits::infinity(); } } @@ -183,8 +185,9 @@ void AbstractPlanningAlgorithm::computeEDTMap() if (isObs(IndexXY{j, i})) { distance = resolution_m; found_obstacle = true; - } else if (found_obstacle && temporary_storage[j] > distance) { - temporary_storage[j] = distance; + } else if (found_obstacle && temporary_storage[j].first > distance) { + temporary_storage[j].first = distance; + temporary_storage[j].second.x = distance; distance += resolution_m; } } @@ -197,22 +200,29 @@ void AbstractPlanningAlgorithm::computeEDTMap() 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]; + double min_value = edt_map[id].first * edt_map[id].first; + geometry_msgs::msg::Point rel_pos = edt_map[id].second; 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; + double value = edt_map[id].first * edt_map[id].first + dist * dist; if (value < min_value) { min_value = value; + rel_pos.x = edt_map[id].second.x; + rel_pos.y = dist; } } - temporary_storage[i] = sqrt(min_value); + temporary_storage[i].first = sqrt(min_value); + temporary_storage[i].second = rel_pos; } for (int i = 0; i < height; ++i) { edt_map[indexToId(IndexXY{j, i})] = temporary_storage[i]; } } - edt_map_ = edt_map; + for (const auto & edt : edt_map) { + const double angle = std::atan2(edt.second.y, edt.second.x); + edt_map_.push_back({edt.first, angle}); + } } void AbstractPlanningAlgorithm::computeCollisionIndexes( @@ -312,7 +322,7 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con if (detectBoundaryExit(base_index)) return true; - double obstacle_edt = getObstacleEDT(base_index); + double obstacle_edt = getObstacleEDT(base_index).distance; // if nearest obstacle is further than largest dimension, no collision is guaranteed // if nearest obstacle is closer than smallest dimension, collision is guaranteed diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index b0be1da75caec..805f375af4276 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -14,11 +14,13 @@ #include "autoware/freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" #include #include +#include #include #include @@ -88,6 +90,9 @@ AstarSearch::AstarSearch( min_expansion_dist_ = astar_param_.expansion_distance; max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; + + near_goal_dist_ = + std::max(astar_param.near_goal_distance, planner_common_param.longitudinal_goal_range); } void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) @@ -100,6 +105,18 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) collision_vehicle_shape_.base_length * base_length_max_expansion_factor_, min_expansion_dist_); } +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_.assign(total_astar_node_count, AstarNode{}); + col_free_distance_map_.assign(nb_of_grid_nodes, std::numeric_limits::max()); + shifted_goal_pose_ = {}; +} + bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) { resetData(); @@ -115,6 +132,8 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) setCollisionFreeDistanceMap(); + is_multiple_goals_ = false; + setStartNode(); if (!search()) { @@ -124,16 +143,54 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) return true; } -void AstarSearch::resetData() +bool AstarSearch::makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) { - // 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()); + if (goal_candidates.empty()) return false; + + if (goal_candidates.size() == 1) { + return makePlan(start_pose, goal_candidates.front()); + } + + resetData(); + + start_pose_ = global2local(costmap_, start_pose); + + std::vector goals_local; + for (const auto & goal : goal_candidates) { + const auto goal_local = global2local(costmap_, goal); + if (detectCollision(goal_local)) continue; + goals_local.push_back(goal_local); + } + + if (detectCollision(start_pose_) || goals_local.empty()) { + throw std::logic_error("Invalid start or goal pose"); + } + + goal_pose_ = is_backward_search_ ? start_pose_ : goals_local.front(); + + setCollisionFreeDistanceMap(); + + is_multiple_goals_ = true; + + if (is_backward_search_) { + double cost_offset = 0.0; + for (const auto & pose : goals_local) { + start_pose_ = pose; + setStartNode(cost_offset); + cost_offset += multi_goal_backward_cost_offset; + } + } else { + setStartNode(); + alternate_goals_ = goals_local; + } + + if (!search()) { + throw std::logic_error("HA* failed to find path to goal"); + } + + return true; } void AstarSearch::setCollisionFreeDistanceMap() @@ -166,7 +223,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; + if (getObstacleEDT(n_index).distance < 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; @@ -177,15 +234,16 @@ void AstarSearch::setCollisionFreeDistanceMap() } } -void AstarSearch::setStartNode() +void AstarSearch::setStartNode(const double cost_offset) { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); // Set start node AstarNode * start_node = &graph_[getKey(index)]; - start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); + const double initial_cost = estimateCost(start_pose_, index) + cost_offset; + start_node->set(start_pose_, 0.0, initial_cost, 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->dist_to_obs = getObstacleEDT(index).distance; start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -261,7 +319,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) 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 auto obs_edt = getObstacleEDT(next_index); const bool is_direction_switch = (current_node.parent != nullptr) && (is_back != current_node.is_back); @@ -271,7 +329,8 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) 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); + move_cost += getObsDistanceCost(next_index, obs_edt); + move_cost += getLatDistanceCost(next_pose); if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance); double total_cost = move_cost + estimateCost(next_pose, next_index); @@ -282,7 +341,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) next_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->dist_to_obs = obs_edt.distance; next_node->parent = ¤t_node; openlist_.push(next_node); continue; @@ -320,10 +379,26 @@ 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 +double AstarSearch::getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const { + if (obs_edt.distance > collision_vehicle_shape_.max_dimension + cost_free_obs_dist) { + return 0.0; + } + const double yaw = index.theta * (2.0 * M_PI / planner_common_param_.theta_size); + const double base_to_frame_dist = getVehicleBaseToFrameDistance(yaw - obs_edt.angle); + const double vehicle_to_obs_dist = std::max(obs_edt.distance - base_to_frame_dist, 0.0); return astar_param_.obstacle_distance_weight * - std::max(1.0 - (obs_distance / cost_free_obs_dist), 0.0); + std::max(1.0 - (vehicle_to_obs_dist / cost_free_obs_dist), 0.0); +} + +double AstarSearch::getLatDistanceCost(const Pose & pose) const +{ + if (is_multiple_goals_) return 0.0; + const auto ref_pose = is_backward_search_ ? start_pose_ : goal_pose_; + const double distance_to_goal = calcDistance2d(pose, ref_pose); + if (distance_to_goal > near_goal_dist_) return 0.0; + const double lat_distance = std::abs(calcRelativePose(ref_pose, pose).position.y); + return astar_param_.goal_lat_distance_weight * lat_distance; } void AstarSearch::setPath(const AstarNode & goal_node) @@ -340,6 +415,11 @@ void AstarSearch::setPath(const AstarNode & goal_node) geometry_msgs::msg::PoseStamped pose; pose.header = header; + if (shifted_goal_pose_) { + pose.pose = local2global(costmap_, shifted_goal_pose_.get()); + waypoints.push_back({pose, goal_node.is_back}); + } + const auto interpolate = [this, &waypoints, &pose](const AstarNode & node) { if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return; const auto parent_pose = node2pose(*node.parent); @@ -375,22 +455,19 @@ void AstarSearch::setPath(const AstarNode & goal_node) } waypoints_.header = header; - waypoints_.waypoints.clear(); + waypoints_.waypoints = waypoints; - for (size_t i = 0; i < waypoints.size() - 1; ++i) { - const auto & current = waypoints[i]; - const auto & next = waypoints[i + 1]; + if (!is_backward_search_) return; - waypoints_.waypoints.push_back(current); + for (size_t i = 0; i < waypoints_.waypoints.size() - 1; ++i) { + const auto & current = waypoints_.waypoints[i]; + auto & next = waypoints_.waypoints[i + 1]; if (current.is_back != next.is_back) { - waypoints_.waypoints.push_back( - {is_backward_search_ ? next.pose : current.pose, - is_backward_search_ ? current.is_back : next.is_back}); + next.is_back = current.is_back; + ++i; // skip next waypoint } } - - waypoints_.waypoints.push_back(waypoints.back()); } bool AstarSearch::isGoal(const AstarNode & node) const @@ -400,26 +477,65 @@ bool AstarSearch::isGoal(const AstarNode & node) const const double goal_angle = autoware::universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); - const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); + const auto node_pose = node2pose(node); - // Check conditions - if (astar_param_.only_behind_solutions && relative_pose.position.x > 0) { - return false; - } + auto checkGoal = [this, &node_pose, &lateral_goal_range, &longitudinal_goal_range, &goal_angle, + &is_back = node.is_back](const Pose & pose) { + const auto node_index = pose2index(costmap_, node_pose, planner_common_param_.theta_size); + const auto goal_index = pose2index(costmap_, pose, planner_common_param_.theta_size); - if ( - std::fabs(relative_pose.position.x) > longitudinal_goal_range || - std::fabs(relative_pose.position.y) > lateral_goal_range) { - return false; - } + if (node_index == goal_index) return true; - const auto angle_diff = - autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); - if (std::abs(angle_diff) > goal_angle) { - return false; - } + const auto relative_pose = calcRelativePose(pose, node_pose); - return true; + bool is_behind_goal = relative_pose.position.x <= 0.0; + + if (astar_param_.only_behind_solutions && !is_behind_goal) { + return false; + } + + if ( + std::fabs(relative_pose.position.x) > longitudinal_goal_range || + std::fabs(relative_pose.position.y) > lateral_goal_range) { + return false; + } + + const auto angle_diff = + autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + if (std::abs(angle_diff) > goal_angle) { + return false; + } + + const bool is_set_shifted_goal_pose = + is_backward_search_ ? is_behind_goal == is_back : is_behind_goal != is_back; + if (is_set_shifted_goal_pose) { + setShiftedGoalPose(pose, relative_pose.position.y); + } + + return true; + }; + + if (checkGoal(goal_pose_)) return true; + + return std::any_of( + alternate_goals_.begin(), alternate_goals_.end(), + [&checkGoal](const Pose & pose) { return checkGoal(pose); }); +} + +void AstarSearch::setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) const +{ + tf2::Transform tf; + tf2::convert(goal_pose, tf); + + geometry_msgs::msg::TransformStamped transform; + transform.transform = tf2::toMsg(tf); + + Pose lat_pose; + lat_pose.position = geometry_msgs::build().x(0.0).y(lat_offset).z(0.0); + lat_pose.orientation = + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0); + + shifted_goal_pose_ = transformPose(lat_pose, transform); } Pose AstarSearch::node2pose(const AstarNode & node) const diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 20a59048e59d1..b9343f680fa1a 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -41,6 +41,13 @@ RRTStar::RRTStar( } } +bool RRTStar::makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) +{ + return makePlan(start_pose, goal_candidates.front()); +} + bool RRTStar::makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & 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 82806bb500b0e..2b2254d66c844 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 @@ -214,12 +214,22 @@ std::unique_ptr configure_astar(bool use_multi) const bool use_back = true; const bool adapt_expansion_distance = true; const double expansion_distance = 0.4; + const double near_goal_distance = 4.0; const double distance_heuristic_weight = 2.0; const double smoothness_weight = 0.5; const double obstacle_distance_weight = 1.7; + const double goal_lat_distance_weight = 1.0; const auto astar_param = fpa::AstarParam{ - search_method, only_behind_solutions, use_back, adapt_expansion_distance, - expansion_distance, distance_heuristic_weight, smoothness_weight, obstacle_distance_weight}; + search_method, + only_behind_solutions, + use_back, + adapt_expansion_distance, + expansion_distance, + near_goal_distance, + distance_heuristic_weight, + smoothness_weight, + obstacle_distance_weight, + goal_lat_distance_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo;