Skip to content

Commit

Permalink
feat(freespace_planning_algorithms): use distance to nearest obstacle…
Browse files Browse the repository at this point in the history
… to improve path planning (#8089)

* refactor freespace planning algorithms

Signed-off-by: mohammad alqudah <[email protected]>

* fix error

Signed-off-by: mohammad alqudah <[email protected]>

* use vector instead of map for a-star node graph

Signed-off-by: mohammad alqudah <[email protected]>

* remove unnecessary parameters

Signed-off-by: mohammad alqudah <[email protected]>

* precompute average turning radius

Signed-off-by: mohammad alqudah <[email protected]>

* add threshold for minimum distance between direction changes

Signed-off-by: mohammad alqudah <[email protected]>

* apply curvature weight and change in curvature weight

Signed-off-by: mohammad alqudah <[email protected]>

* store total cost instead of heuristic cost

Signed-off-by: mohammad alqudah <[email protected]>

* fix reverse weight application

Signed-off-by: mohammad alqudah <[email protected]>

* fix parameter description in README

Signed-off-by: mohammad alqudah <[email protected]>

* implement edt map to store distance to nearest obstacle for each grid cell

Signed-off-by: mohammad alqudah <[email protected]>

* use obstacle edt in collision check

Signed-off-by: mohammad alqudah <[email protected]>

* add cost for distance to obstacle

Signed-off-by: mohammad alqudah <[email protected]>

* fix formats

Signed-off-by: mohammad alqudah <[email protected]>

* add missing include

Signed-off-by: mohammad alqudah <[email protected]>

* refactor functions

Signed-off-by: mohammad alqudah <[email protected]>

* add missing include

Signed-off-by: mohammad alqudah <[email protected]>

* precompute number of margin cells to reduce out of range vertices check necessity

Signed-off-by: mohammad alqudah <[email protected]>

* add reset data function

Signed-off-by: mohammad alqudah <[email protected]>

* add member function set() to AstarNode struct

Signed-off-by: mohammad alqudah <[email protected]>

* implement adaptive expansion distance

Signed-off-by: mohammad alqudah <[email protected]>

* remove unnecessary code

Signed-off-by: mohammad alqudah <[email protected]>

* interpolate nodes with large expansion distance

Signed-off-by: mohammad alqudah <[email protected]>

* minor refactor

Signed-off-by: mohammad alqudah <[email protected]>

* ensure expansion distance is larger than grid cell diagonal

Signed-off-by: mohammad alqudah <[email protected]>

* compute collision free distance to goal map

Signed-off-by: mohammad alqudah <[email protected]>

* use obstacle edt when computing collision free distance map

Signed-off-by: mohammad alqudah <[email protected]>

* minor refactor

Signed-off-by: mohammad alqudah <[email protected]>

* fix expansion cost function

Signed-off-by: mohammad alqudah <[email protected]>

* set distance map before setting start node

Signed-off-by: mohammad alqudah <[email protected]>

* refactor detect collision function

Signed-off-by: mohammad alqudah <[email protected]>

* add missing variable initialization

Signed-off-by: mohammad alqudah <[email protected]>

* remove declared but undefined function

Signed-off-by: mohammad alqudah <[email protected]>

* remove unnecessary checks

Signed-off-by: mohammad alqudah <[email protected]>

* minor fix

Signed-off-by: mohammad alqudah <[email protected]>

* refactor computeEDTMap function

Signed-off-by: mohammad alqudah <[email protected]>

* remove unnecessary code

Signed-off-by: mohammad alqudah <[email protected]>

* set min and max expansion distance after setting costmap

Signed-off-by: mohammad alqudah <[email protected]>

* refactor detectCollision function

Signed-off-by: mohammad alqudah <[email protected]>

* remove unused function

Signed-off-by: mohammad alqudah <[email protected]>

* change default parameter values

Signed-off-by: mohammad alqudah <[email protected]>

* fix computeEDTMap function

Signed-off-by: mohammad alqudah <[email protected]>

* rename parameter

Signed-off-by: mohammad alqudah <[email protected]>

* use linear function for obstacle distance cost

Signed-off-by: mohammad alqudah <[email protected]>

* fix rrtstar obstacle check

Signed-off-by: mohammad alqudah <[email protected]>

* remove redundant return statements

Signed-off-by: mohammad alqudah <[email protected]>

* check goal pose validity before setting collision free distance map

Signed-off-by: mohammad alqudah <[email protected]>

* declare variables as const where necessary

Signed-off-by: mohammad alqudah <[email protected]>

* compare front and back lengths when setting min and max dimension

Signed-off-by: mohammad alqudah <[email protected]>

* add docstring and citation for computeEDTMap function

Signed-off-by: mohammad alqudah <[email protected]>

* suppress spell check

Signed-off-by: mohammad alqudah <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>
Co-authored-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
mkquda and maxime-clem authored Aug 14, 2024
1 parent 31263bd commit 2a050d7
Show file tree
Hide file tree
Showing 12 changed files with 250 additions and 135 deletions.
2 changes: 2 additions & 0 deletions planning/autoware_freespace_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <tf2/utils.h>

#include <algorithm>
#include <limits>
#include <vector>

namespace autoware::freespace_planning_algorithms
Expand Down Expand Up @@ -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;

Expand All @@ -74,6 +77,7 @@ struct VehicleShape
max_steering(max_steering),
base2back(base2back)
{
setMinMaxDimension();
}

explicit VehicleShape(
Expand All @@ -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);
}
}
};

Expand Down Expand Up @@ -156,6 +173,16 @@ class AbstractPlanningAlgorithm
std::vector<IndexXY> & 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 <typename IndexType>
inline bool isOutOfRange(const IndexType & index) const
Expand Down Expand Up @@ -194,6 +221,12 @@ class AbstractPlanningAlgorithm
return is_obstacle_table_[indexToId(index)];
}

template <typename IndexType>
inline double getObstacleEDT(const IndexType & index) const
{
return edt_map_[indexToId(index)];
}

// compute single dimensional grid cell index from 2 dimensional index
template <typename IndexType>
inline int indexToId(const IndexType & index) const
Expand All @@ -216,6 +249,9 @@ class AbstractPlanningAlgorithm
// is_obstacle's table
std::vector<bool> is_obstacle_table_;

// Euclidean distance transform map (distance to nearest obstacle cell)
std::vector<double> edt_map_;

// pose in costmap frame
geometry_msgs::msg::Pose start_pose_;
geometry_msgs::msg::Pose goal_pose_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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<std::vector<NodeUpdate>>;

AstarSearch(
const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
const AstarParam & astar_param);
Expand All @@ -134,12 +101,15 @@ class AstarSearch : public AbstractPlanningAlgorithm
AstarParam{
node.declare_parameter<bool>("astar.only_behind_solutions"),
node.declare_parameter<bool>("astar.use_back"),
node.declare_parameter<bool>("astar.adapt_expansion_distance"),
node.declare_parameter<double>("astar.expansion_distance"),
node.declare_parameter<double>("astar.distance_heuristic_weight"),
node.declare_parameter<double>("astar.smoothness_weight")})
node.declare_parameter<double>("astar.smoothness_weight"),
node.declare_parameter<double>("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_; }
Expand All @@ -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<AstarNode> graph_;
std::vector<double> col_free_distance_map_;

Expand All @@ -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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_<freespace_planning_algorithms::PlannerCommonParam>(
p, "PlannerCommonParam", py::dynamic_attr())
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
Loading

0 comments on commit 2a050d7

Please sign in to comment.