Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(safety_check): add option to use polygon along path in safety check #6336

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,7 @@
time_horizon_for_rear_object: 10.0 # [s]
delay_until_departure: 0.0 # [s]
# rss parameters
extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path"
expected_front_deceleration: -1.0 # [m/ss]
expected_rear_deceleration: -1.0 # [m/ss]
rear_vehicle_reaction_time: 2.0 # [s]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -223,6 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
// safety check rss params
{
const std::string ns = "avoidance.safety_check.";
p.rss_params.extended_polygon_policy =
getOrDeclareParameter<std::string>(*node, ns + "extended_polygon_policy");
p.rss_params.longitudinal_distance_min_threshold =
getOrDeclareParameter<double>(*node, ns + "longitudinal_distance_min_threshold");
p.rss_params.longitudinal_velocity_delta_time =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,9 @@ struct TargetObjectsOnLane
*/
struct RSSparams
{
std::string extended_polygon_policy{
"rectangle"}; ///< Policy to create collision check polygon.
///< possible values: ["rectangle", "along_path"]
double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle.
double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle.
double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance.
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession

The ratio of primitive types in function arguments increases from 33.73% to 34.44%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -184,6 +184,88 @@
: tier4_autoware_utils::inverseClockwise(polygon);
}

Polygon2d createExtendedPolygonAlongPath(

Check warning on line 187 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L187

Added line #L187 was not covered by tests
const PathWithLaneId & planned_path, const Pose & base_link_pose,
const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length,
const double lat_margin, const double is_stopped_obj, CollisionCheckDebug & debug)
{
const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
const double & width = vehicle_info.vehicle_width_m;
const double & base_to_rear = vehicle_info.rear_overhang_m;

// if stationary object, extend forward and backward by the half of lon length
const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length);
const double backward_lon_offset =

Check warning on line 198 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L198

Added line #L198 was not covered by tests
-base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value
const double lat_offset = width / 2.0 + lat_margin;

Check warning on line 200 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L200

Added line #L200 was not covered by tests

{
debug.forward_lon_offset = forward_lon_offset;
debug.backward_lon_offset = backward_lon_offset;
debug.lat_offset = lat_offset;

Check warning on line 205 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L203-L205

Added lines #L203 - L205 were not covered by tests
}

const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose(
planned_path.points, base_link_pose.position, lon_length);

Check warning on line 209 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L208-L209

Added lines #L208 - L209 were not covered by tests
if (!lon_offset_pose.has_value()) {
return createExtendedPolygon(
base_link_pose, vehicle_info, lon_length, lat_margin, is_stopped_obj, debug);

Check warning on line 212 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L212

Added line #L212 was not covered by tests
}

const size_t start_idx =
motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position);

Check warning on line 216 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L216

Added line #L216 was not covered by tests
const size_t end_idx =
motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position);

Check warning on line 218 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L218

Added line #L218 was not covered by tests

Polygon2d polygon;

{
const auto p_offset =
tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

for (size_t i = start_idx + 1; i < end_idx + 1; ++i) {
const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i));
const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

{
const auto p_offset =
tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

{
const auto p_offset = tier4_autoware_utils::calcOffsetPose(
lon_offset_pose.value(), base_to_front, -lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

for (size_t i = end_idx; i > start_idx; --i) {
const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i));
const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

{
const auto p_offset =
tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

{
const auto p_offset =
tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);
appendPointToPolygon(polygon, p_offset.position);
}

return tier4_autoware_utils::isClockwise(polygon)
? polygon
: tier4_autoware_utils::inverseClockwise(polygon);
}

Check notice on line 267 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Excess Number of Function Arguments

createExtendedPolygonAlongPath has 7 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check warning on line 267 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L267

Added line #L267 was not covered by tests

std::vector<Polygon2d> createExtendedPolygonsFromPoseWithVelocityStamped(
const std::vector<PoseWithVelocityStamped> & predicted_path, const VehicleInfo & vehicle_info,
const double forward_margin, const double backward_margin, const double lat_margin)
Expand Down Expand Up @@ -549,10 +631,24 @@
const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor;
// TODO(watanabe) fix hard coding value
const bool is_stopped_object = object_velocity < 0.3;
const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(
ego_pose, ego_vehicle_info, lon_offset,
lat_margin, is_stopped_object, debug)
: ego_polygon;
const auto extended_ego_polygon = [&]() {

Check warning on line 634 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L634

Added line #L634 was not covered by tests
if (!is_object_front) {
return ego_polygon;

Check warning on line 636 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L636

Added line #L636 was not covered by tests
}

if (rss_parameters.extended_polygon_policy == "rectangle") {
return createExtendedPolygon(
ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, debug);

Check warning on line 641 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L641

Added line #L641 was not covered by tests
}

if (rss_parameters.extended_polygon_policy == "along_path") {
return createExtendedPolygonAlongPath(
planned_path, ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object,
debug);

Check warning on line 647 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp#L646-L647

Added lines #L646 - L647 were not covered by tests
}

throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'.");
}();

Check warning on line 651 in planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

getCollidedPolygons increases in cyclomatic complexity from 10 to 13, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
const auto & extended_obj_polygon =
is_object_front
? obj_polygon
Expand Down
Loading