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.

Sign up for GitHub

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

fix(lane_change): fix chattering for stopped objects #5302

Merged
merged 1 commit into from
Oct 15, 2023
Merged

fix(lane_change): fix chattering for stopped objects #5302

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
25 changes: 13 additions & 12 deletions ...nclude/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,18 +182,19 @@ struct SafetyCheckParams

struct CollisionCheckDebug
{
std::string unsafe_reason; ///< Reason indicating unsafe situation.
Twist current_twist{}; ///< Ego vehicle's current velocity and rotation.
Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle.
Pose current_obj_pose{}; ///< Detected object's current pose.
Twist object_twist{}; ///< Detected object's velocity and rotation.
Pose expected_obj_pose{}; ///< Predicted future pose of object.
double rss_longitudinal{0.0}; ///< Longitudinal RSS measure.
double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object.
double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon.
double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon.
bool is_front{false}; ///< True if object is in front of ego vehicle.
bool is_safe{false}; ///< True if situation is deemed safe.
std::string unsafe_reason; ///< Reason indicating unsafe situation.
Twist current_twist{}; ///< Ego vehicle's current velocity and rotation.
Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle.
Pose current_obj_pose{}; ///< Detected object's current pose.
Twist object_twist{}; ///< Detected object's velocity and rotation.
Pose expected_obj_pose{}; ///< Predicted future pose of object.
double rss_longitudinal{0.0}; ///< Longitudinal RSS measure.
double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object.
double forward_lon_offset{0.0}; ///< Forward longitudinal offset for extended polygon.
double backward_lon_offset{0.0}; ///< Backward longitudinal offset for extended polygon.
double lat_offset{0.0}; ///< Lateral offset for extended polygon.
bool is_front{false}; ///< True if object is in front of ego vehicle.
bool is_safe{false}; ///< True if situation is deemed safe.
std::vector<PoseWithVelocityStamped> ego_predicted_path; ///< ego vehicle's predicted path.
std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path; ///< object's predicted path.
Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon.
Expand Down
5 changes: 3 additions & 2 deletions ...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,10 +66,11 @@ bool isTargetObjectFront(

Polygon2d createExtendedPolygon(
const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
const double lon_length, const double lat_margin, CollisionCheckDebug & debug);
const double lon_length, const double lat_margin, const double is_stopped_obj,
CollisionCheckDebug & debug);
Polygon2d createExtendedPolygon(
const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
CollisionCheckDebug & debug);
const double is_stopped_obj, CollisionCheckDebug & debug);

PredictedPath convertToPredictedPath(
const std::vector<PoseWithVelocityStamped> & path, const double time_resolution);
Expand Down
5 changes: 3 additions & 2 deletions planning/behavior_path_planner/src/marker_utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -641,8 +641,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st
<< "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
<< "\nEgo to obj: " << info.inter_vehicle_distance
<< "\nExtended polygon: " << (info.is_front ? "ego" : "object")
<< "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
<< "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
<< "\nExtended polygon lateral offset: " << info.lat_offset
<< "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset
<< "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset
<< "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego")
<< "\nSafe: " << (info.is_safe ? "Yes" : "No");

Expand Down
60 changes: 38 additions & 22 deletions planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
View file
Open in desktop
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/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 35.29% to 37.74%, 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 @@ -82,28 +82,33 @@

Polygon2d createExtendedPolygon(
const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
const double lon_length, const double lat_margin, CollisionCheckDebug & debug)
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;

const double lon_offset = std::max(lon_length + base_to_front, base_to_front);

// 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 =
-base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value
const double lat_offset = width / 2.0 + lat_margin;

{
debug.extended_polygon_lon_offset = lon_offset;
debug.extended_polygon_lat_offset = lat_offset;
debug.forward_lon_offset = forward_lon_offset;
debug.backward_lon_offset = backward_lon_offset;
debug.lat_offset = lat_offset;
}

const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0);
const auto p1 =
tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0);
const auto p2 =
tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0);
tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0);
const auto p3 =
tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0);
tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0);
const auto p4 =
tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0);
tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);

Check notice on line 111 in planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Excess Number of Function Arguments 

createExtendedPolygon increases from 5 to 6 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Polygon2d polygon;
appendPointToPolygon(polygon, p1.position);
Expand All @@ -118,40 +123,48 @@

Polygon2d createExtendedPolygon(
const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
CollisionCheckDebug & debug)
const double is_stopped_obj, CollisionCheckDebug & debug)
{
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape);
if (obj_polygon.outer().empty()) {
return obj_polygon;
}

double max_x = std::numeric_limits<double>::lowest();
double min_x = std::numeric_limits<double>::max();
double max_y = std::numeric_limits<double>::lowest();
double min_y = std::numeric_limits<double>::max();
for (const auto & polygon_p : obj_polygon.outer()) {
const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose);

max_x = std::max(transformed_p.x, max_x);
min_x = std::min(transformed_p.x, min_x);
max_y = std::max(transformed_p.y, max_y);
min_y = std::min(transformed_p.y, min_y);
}

const double lon_offset = max_x + lon_length;
// if stationary object, extend forward and backward by the half of lon length
const double forward_lon_offset = max_x + (is_stopped_obj ? lon_length / 2 : lon_length);
const double backward_lon_offset = min_x - (is_stopped_obj ? lon_length / 2 : 0); // minus value

const double left_lat_offset = max_y + lat_margin;
const double right_lat_offset = min_y - lat_margin;

{
debug.extended_polygon_lon_offset = lon_offset;
debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2;
debug.forward_lon_offset = forward_lon_offset;
debug.backward_lon_offset = backward_lon_offset;
debug.lat_offset = (left_lat_offset + right_lat_offset) / 2;
}

const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0);
const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0);
const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0);
const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0);
const auto p1 =
tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0);
const auto p2 =
tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0);
const auto p3 =
tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0);
const auto p4 =
tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0);

Check notice on line 167 in planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Excess Number of Function Arguments 

createExtendedPolygon increases from 5 to 6 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Polygon2d polygon;
appendPointToPolygon(polygon, p1.position);
Expand Down Expand Up @@ -338,14 +351,17 @@

const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor;
const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor;
const auto & extended_ego_polygon =
is_object_front
? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug)
: ego_polygon;
// TODO(watanabe) fix hard coding value
const bool is_stopped_object = object_velocity < 0.3;

Check warning on line 355 in planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

View check run for this annotation

Codecov / codecov/patch

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

Added line #L355 was not covered by tests
rej55 marked this conversation as resolved.
Show resolved Hide resolved
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_obj_polygon =
is_object_front
? obj_polygon
: createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug);
: createExtendedPolygon(
obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug);

Check warning on line 364 in planning/behavior_path_planner/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 already has high cyclomatic complexity, and now it increases in Lines of Code from 72 to 74. 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.

{
debug.rss_longitudinal = rss_dist;
Expand Down
19 changes: 12 additions & 7 deletions planning/behavior_path_planner/test/test_safety_check.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,64 +52,67 @@

const double lon_length = 10.0;
const double lat_margin = 2.0;
const bool is_stopped_object = false;

const auto polygon =
createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug);
const auto polygon = createExtendedPolygon(
ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug);

EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));

const auto & p1 = polygon.outer().at(0);
const auto & p2 = polygon.outer().at(1);
const auto & p3 = polygon.outer().at(2);
const auto & p4 = polygon.outer().at(3);
EXPECT_NEAR(p1.x(), 14.0, epsilon);
EXPECT_NEAR(p1.y(), 3.0, epsilon);
EXPECT_NEAR(p2.x(), 14.0, epsilon);
EXPECT_NEAR(p2.y(), -3.0, epsilon);
EXPECT_NEAR(p3.x(), -1.0, epsilon);
EXPECT_NEAR(p3.y(), -3.0, epsilon);
EXPECT_NEAR(p4.x(), -1.0, epsilon);
EXPECT_NEAR(p4.y(), 3.0, epsilon);
}

{
Pose ego_pose;
ego_pose.position = tier4_autoware_utils::createPoint(3.0, 4.0, 0.0);
ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0);

const double lon_length = 10.0;
const double lat_margin = 2.0;
const bool is_stopped_object = false;

const auto polygon =
createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug);
const auto polygon = createExtendedPolygon(
ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug);

EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));

const auto & p1 = polygon.outer().at(0);
const auto & p2 = polygon.outer().at(1);
const auto & p3 = polygon.outer().at(2);
const auto & p4 = polygon.outer().at(3);
EXPECT_NEAR(p1.x(), 17.0, epsilon);
EXPECT_NEAR(p1.y(), 7.0, epsilon);
EXPECT_NEAR(p2.x(), 17.0, epsilon);
EXPECT_NEAR(p2.y(), 1.0, epsilon);
EXPECT_NEAR(p3.x(), 2.0, epsilon);
EXPECT_NEAR(p3.y(), 1.0, epsilon);
EXPECT_NEAR(p4.x(), 2.0, epsilon);
EXPECT_NEAR(p4.y(), 7.0, epsilon);
}

{
Pose ego_pose;
ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0);
ego_pose.orientation =
tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::deg2rad(60));

const double lon_length = 10.0;
const double lat_margin = 2.0;
const bool is_stopped_object = false;

const auto polygon =
createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug);
const auto polygon = createExtendedPolygon(
ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug);

Check warning on line 115 in planning/behavior_path_planner/test/test_safety_check.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

TEST:BehaviorPathPlanningSafetyUtilsTest:createExtendedEgoPolygon increases from 76 to 79 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));

Expand Down Expand Up @@ -155,9 +158,11 @@

const double lon_length = 10.0;
const double lat_margin = 2.0;
const bool is_stopped_object = false;

CollisionCheckDebug debug;
const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug);
const auto polygon =
createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug);

EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));

Expand Down
Loading