Skip to content

Commit 9aa2e5a

Browse files
feat(goal_planner): secure lateral distance for path planning with corresponding to the centrifugal force (autowarefoundation#5196)
* add feature to plan the path with considering latral overshoot Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * separate a function in "safety_check" in order to that "goal_planner_module" can handle the ego_polygons Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * add visualization codes fix the handling miss of sign of value Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * refactor codes based on comments in the PR Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
1 parent 1c2c59d commit 9aa2e5a

File tree

4 files changed

+82
-36
lines changed

4 files changed

+82
-36
lines changed

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,7 @@ struct FreespacePlannerDebugData
106106
struct GoalPlannerDebugData
107107
{
108108
FreespacePlannerDebugData freespace_planner{};
109+
std::vector<Polygon2d> ego_polygons_expanded{};
109110
};
110111

111112
class GoalPlannerModule : public SceneModuleInterface

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

+8-6
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,10 @@ bool checkCollision(
110110
const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
111111
const double hysteresis_factor, CollisionCheckDebug & debug);
112112

113+
std::vector<Polygon2d> generatePolygonsWithStoppingAndInertialMargin(
114+
const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear,
115+
const double width, const double maximum_deceleration, const double max_extra_stopping_margin);
116+
113117
/**
114118
* @brief Iterate the points in the ego and target's predicted path and
115119
* perform safety check for each of the iterated points.
@@ -133,14 +137,12 @@ std::vector<Polygon2d> getCollidedPolygons(
133137
const double hysteresis_factor, CollisionCheckDebug & debug);
134138

135139
/**
136-
* @brief Check collision between ego path footprints with extra longitudinal stopping margin and
137-
* objects.
140+
* @brief Check collision between ego polygons with margin and objects.
138141
* @return Has collision or not
139142
*/
140-
bool checkCollisionWithExtraStoppingMargin(
141-
const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
142-
const double base_to_front, const double base_to_rear, const double width,
143-
const double maximum_deceleration, const double margin, const double max_stopping_margin);
143+
bool checkCollisionWithMargin(
144+
const std::vector<Polygon2d> & ego_polygons, const PredictedObjects & dynamic_objects,
145+
const double collision_check_margin);
144146
} // namespace behavior_path_planner::utils::path_safety_checker
145147

146148
#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

+42-21
Original file line numberDiff line numberDiff line change
@@ -1296,28 +1296,32 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
12961296
}
12971297
}
12981298

1299-
if (parameters_->use_object_recognition) {
1300-
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
1301-
*(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
1302-
parameters_->forward_goal_search_length);
1303-
const auto [pull_over_lane_objects, others] =
1304-
utils::path_safety_checker::separateObjectsByLanelets(
1305-
*(planner_data_->dynamic_object), pull_over_lanes);
1306-
const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
1307-
pull_over_lane_objects, parameters_->th_moving_object_velocity);
1308-
const auto common_parameters = planner_data_->parameters;
1309-
const double base_link2front = common_parameters.base_link2front;
1310-
const double base_link2rear = common_parameters.base_link2rear;
1311-
const double vehicle_width = common_parameters.vehicle_width;
1312-
if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin(
1313-
path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width,
1314-
parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin,
1315-
parameters_->object_recognition_collision_check_max_extra_stopping_margin)) {
1316-
return true;
1317-
}
1299+
if (!parameters_->use_object_recognition) {
1300+
return false;
13181301
}
13191302

1320-
return false;
1303+
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
1304+
*(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
1305+
parameters_->forward_goal_search_length);
1306+
const auto [pull_over_lane_objects, others] =
1307+
utils::path_safety_checker::separateObjectsByLanelets(
1308+
*(planner_data_->dynamic_object), pull_over_lanes);
1309+
const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
1310+
pull_over_lane_objects, parameters_->th_moving_object_velocity);
1311+
const auto common_parameters = planner_data_->parameters;
1312+
const double base_link2front = common_parameters.base_link2front;
1313+
const double base_link2rear = common_parameters.base_link2rear;
1314+
const double vehicle_width = common_parameters.vehicle_width;
1315+
1316+
const auto ego_polygons_expanded =
1317+
utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin(
1318+
path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration,
1319+
parameters_->object_recognition_collision_check_max_extra_stopping_margin);
1320+
debug_data_.ego_polygons_expanded = ego_polygons_expanded;
1321+
1322+
return utils::path_safety_checker::checkCollisionWithMargin(
1323+
ego_polygons_expanded, pull_over_lane_stop_objects,
1324+
parameters_->object_recognition_collision_check_margin);
13211325
}
13221326

13231327
bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const
@@ -1686,6 +1690,24 @@ void GoalPlannerModule::setDebugData()
16861690
add(
16871691
createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9));
16881692
}
1693+
1694+
auto marker = tier4_autoware_utils::createDefaultMarker(
1695+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST,
1696+
tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0),
1697+
tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999));
1698+
1699+
for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) {
1700+
for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) {
1701+
const auto & current_point = ego_polygon.outer().at(ep_idx);
1702+
const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size());
1703+
1704+
marker.points.push_back(
1705+
tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0));
1706+
marker.points.push_back(
1707+
tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0));
1708+
}
1709+
}
1710+
debug_marker_.markers.push_back(marker);
16891711
}
16901712
// safety check
16911713
if (parameters_->safety_check_params.enable_safety_check) {
@@ -1695,7 +1717,6 @@ void GoalPlannerModule::setDebugData()
16951717
add(createPredictedPathMarkerArray(
16961718
ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9));
16971719
}
1698-
16991720
if (goal_planner_data_.filtered_objects.objects.size() > 0) {
17001721
add(createObjectsMarkerArray(
17011722
goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));

planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

+31-9
Original file line numberDiff line numberDiff line change
@@ -364,27 +364,49 @@ std::vector<Polygon2d> getCollidedPolygons(
364364

365365
return collided_polygons;
366366
}
367-
bool checkCollisionWithExtraStoppingMargin(
368-
const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
369-
const double base_to_front, const double base_to_rear, const double width,
370-
const double maximum_deceleration, const double collision_check_margin,
371-
const double max_extra_stopping_margin)
367+
368+
std::vector<Polygon2d> generatePolygonsWithStoppingAndInertialMargin(
369+
const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear,
370+
const double width, const double maximum_deceleration, const double max_extra_stopping_margin)
372371
{
373-
for (const auto & p : ego_path.points) {
372+
std::vector<Polygon2d> polygons;
373+
const auto curvatures = motion_utils::calcCurvature(ego_path.points);
374+
375+
for (size_t i = 0; i < ego_path.points.size(); ++i) {
376+
const auto p = ego_path.points.at(i);
377+
374378
const double extra_stopping_margin = std::min(
375379
std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration,
376380
max_extra_stopping_margin);
377381

382+
double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps *
383+
std::abs(p.point.longitudinal_velocity_mps);
384+
extra_lateral_margin =
385+
std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin);
386+
387+
const auto lateral_offset_pose =
388+
tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0);
378389
const auto ego_polygon = tier4_autoware_utils::toFootprint(
379-
p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width);
390+
lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear,
391+
width + std::abs(extra_lateral_margin));
392+
polygons.push_back(ego_polygon);
393+
}
394+
return polygons;
395+
}
380396

397+
bool checkCollisionWithMargin(
398+
const std::vector<Polygon2d> & ego_polygons, const PredictedObjects & dynamic_objects,
399+
const double collision_check_margin)
400+
{
401+
for (const auto & ego_polygon : ego_polygons) {
381402
for (const auto & object : dynamic_objects.objects) {
382403
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
383404
const double distance = boost::geometry::distance(obj_polygon, ego_polygon);
384-
if (distance < collision_check_margin) return true;
405+
if (distance < collision_check_margin) {
406+
return true;
407+
}
385408
}
386409
}
387-
388410
return false;
389411
}
390412
} // namespace behavior_path_planner::utils::path_safety_checker

0 commit comments

Comments
 (0)