Skip to content

Commit 74e424b

Browse files
committed
add option to ignore unavoidable collisions
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 1ca97cf commit 74e424b

File tree

5 files changed

+38
-11
lines changed

5 files changed

+38
-11
lines changed

planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -8,3 +8,4 @@
88
hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection
99
decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled
1010
minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision
11+
ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight)

planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node
3939
getOrDeclareParameter<double>(node, ns + ".decision_duration_buffer");
4040
pp.minimum_object_distance_from_ego_path =
4141
getOrDeclareParameter<double>(node, ns + ".minimum_object_distance_from_ego_path");
42+
pp.ignore_unavoidable_collisions =
43+
getOrDeclareParameter<bool>(node, ns + ".ignore_unavoidable_collisions");
4244

4345
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
4446
pp.ego_lateral_offset =

planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

+23-2
Original file line numberDiff line numberDiff line change
@@ -59,13 +59,34 @@ std::vector<autoware_auto_perception_msgs::msg::PredictedObject> filter_predicte
5959
return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx +
6060
params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0;
6161
};
62-
for (const auto & object : objects.objects)
62+
const auto is_unavoidable = [&](const auto & o) {
63+
const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose;
64+
const auto o_yaw = tf2::getYaw(o_pose.orientation);
65+
const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation);
66+
const auto yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(o_yaw - ego_yaw));
67+
const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4;
68+
auto distance = std::abs(
69+
(o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) -
70+
(o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw));
71+
if (ego_data.earliest_stop_pose) {
72+
distance = std::min(
73+
distance,
74+
std::abs(
75+
(o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) -
76+
(o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)));
77+
}
78+
return opposite_heading &&
79+
distance <= params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis;
80+
};
81+
for (const auto & object : objects.objects) {
6382
if (
6483
is_vehicle(object) &&
6584
object.kinematics.initial_twist_with_covariance.twist.linear.x >=
6685
params.minimum_object_velocity &&
67-
is_in_range(object) && is_not_too_close(object))
86+
is_in_range(object) && is_not_too_close(object) &&
87+
(!params.ignore_unavoidable_collisions || !is_unavoidable(object)))
6888
filtered_objects.push_back(object);
89+
}
6990
return filtered_objects;
7091
}
7192
} // namespace behavior_velocity_planner::dynamic_obstacle_stop

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

+10-9
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,15 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
6565
motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
6666
ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment(
6767
ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position);
68+
const auto min_stop_distance =
69+
motion_utils::calcDecelDistWithJerkAndAccConstraints(
70+
planner_data_->current_velocity->twist.linear.x, 0.0,
71+
planner_data_->current_acceleration->accel.accel.linear.x,
72+
planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold,
73+
planner_data_->max_stop_jerk_threshold)
74+
.value_or(0.0);
75+
ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose(
76+
ego_data.path.points, ego_data.pose.position, min_stop_distance);
6877

6978
make_ego_footprint_rtree(ego_data, params_);
7079
const auto has_decided_to_stop =
@@ -80,13 +89,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
8089
const auto obstacle_forward_footprints =
8190
make_forward_footprints(dynamic_obstacles, params_, hysteresis);
8291
const auto footprints_duration_us = stopwatch.toc("footprints");
83-
const auto min_stop_distance =
84-
motion_utils::calcDecelDistWithJerkAndAccConstraints(
85-
planner_data_->current_velocity->twist.linear.x, 0.0,
86-
planner_data_->current_acceleration->accel.accel.linear.x,
87-
planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold,
88-
planner_data_->max_stop_jerk_threshold)
89-
.value_or(0.0);
9092
stopwatch.tic("collisions");
9193
const auto collision =
9294
find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_);
@@ -101,8 +103,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
101103
? motion_utils::calcLongitudinalOffsetPose(
102104
ego_data.path.points, *collision,
103105
-params_.stop_distance_buffer - params_.ego_longitudinal_offset)
104-
: motion_utils::calcLongitudinalOffsetPose(
105-
ego_data.path.points, ego_data.pose.position, min_stop_distance);
106+
: ego_data.earliest_stop_pose;
106107
if (stop_pose) {
107108
const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength(
108109
path->points, stop_pose->position,

planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ struct PlannerParam
4444
double ego_longitudinal_offset;
4545
double ego_lateral_offset;
4646
double minimum_object_distance_from_ego_path;
47+
bool ignore_unavoidable_collisions;
4748
};
4849

4950
struct EgoData
@@ -54,6 +55,7 @@ struct EgoData
5455
geometry_msgs::msg::Pose pose;
5556
tier4_autoware_utils::MultiPolygon2d path_footprints;
5657
Rtree rtree;
58+
std::optional<geometry_msgs::msg::Pose> earliest_stop_pose;
5759
};
5860

5961
/// @brief debug data

0 commit comments

Comments
 (0)