Skip to content

Commit

Permalink
fix(lane_change): fix chattering for stopped objects
Browse files Browse the repository at this point in the history 
Signed-off-by: kosuke55 <[email protected]>

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

fix(path_safety_checker): remove redundant parameter name

Signed-off-by: Fumiya Watanabe <[email protected]>
  • Loading branch information
@kosuke55 @TakaHoribe
kosuke55 authored and TakaHoribe committed Oct 15, 2023
1 parent e3da837 commit f414ae5
Show file tree
Hide file tree
Showing 5 changed files with 69 additions and 45 deletions.
25 changes: 13 additions & 12 deletions ...nclude/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,18 +179,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
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,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
Original file line number Diff line number Diff line change
Expand Up @@ -636,8 +636,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
Original file line number Diff line number Diff line change
Expand Up @@ -70,28 +70,33 @@ 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)
{
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);

Polygon2d polygon;
appendPointToPolygon(polygon, p1.position);
Expand All @@ -106,7 +111,7 @@ Polygon2d createExtendedPolygon(

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()) {
Expand All @@ -127,19 +132,27 @@ Polygon2d createExtendedPolygon(
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);

Polygon2d polygon;
appendPointToPolygon(polygon, p1.position);
Expand Down Expand Up @@ -326,14 +339,17 @@ std::vector<Polygon2d> getCollidedPolygons(

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;
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);

{
debug.rss_longitudinal = rss_dist;
Expand Down
19 changes: 12 additions & 7 deletions planning/behavior_path_planner/test/test_safety_check.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)

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));

Expand All @@ -78,9 +79,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)

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));

Expand All @@ -106,9 +108,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)

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));

Expand Down Expand Up @@ -154,9 +157,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon)

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

0 comments on commit f414ae5

Please sign in to comment.