From 8d5042d8a563685917cf6808676790b84f71abdc Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 19 Mar 2024 16:08:33 +0900 Subject: [PATCH] feat(dynamic_obstacle_stop): add option to ignore unavoidable collisions (#6594) Signed-off-by: Maxime CLEMENT --- .../README.md | 22 +++++++----- .../config/dynamic_obstacle_stop.param.yaml | 1 + .../src/manager.cpp | 2 ++ .../src/object_filtering.cpp | 34 ++++++++++++++++--- .../src/scene_dynamic_obstacle_stop.cpp | 28 ++++++++------- .../src/types.hpp | 2 ++ 6 files changed, 63 insertions(+), 26 deletions(-) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md index 8aa3415c3f329..5683295332dc2 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md @@ -38,8 +38,11 @@ An object is considered by the module only if it meets all of the following cond - it is a vehicle (pedestrians are ignored); - it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; - it is not too close to the current position of the ego vehicle; +- it is not unavoidable (only if parameter `ignore_unavoidable_collisions` is set to `true`); - it is close to the ego path. +An object is considered unavoidable if it is heading towards the ego vehicle such that even if ego stops, a collision would still occur (assuming the object keeps driving in a straight line). + For the last condition, the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. @@ -74,12 +77,13 @@ the stop point arc length is calculated to be the arc length of the previously f ### Module Parameters -| Parameter | Type | Description | -| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | -| `extra_object_width` | double | [m] extra width around detected objects | -| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | -| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | -| `time_horizon` | double | [s] time horizon used for collision checks | -| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | -| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | -| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | +| Parameter | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | +| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | +| `ignore_unavoidable_collisions` | bool | [-] if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml index 14483093e8bb3..c18de5bd53f01 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -8,3 +8,4 @@ hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision + ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 99981007b20c2..845070347d927 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -39,6 +39,8 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node getOrDeclareParameter(node, ns + ".decision_duration_buffer"); pp.minimum_object_distance_from_ego_path = getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); + pp.ignore_unavoidable_collisions = + getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.ego_lateral_offset = diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 0411ab2d01cfe..c95db485781f9 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -59,13 +59,37 @@ std::vector filter_predicte return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; }; - for (const auto & object : objects.objects) + const auto is_unavoidable = [&](const auto & o) { + const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose; + const auto o_yaw = tf2::getYaw(o_pose.orientation); + const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation); + const auto yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(o_yaw - ego_yaw)); + const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4; + const auto collision_distance_threshold = + params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis; + // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_point_and_angle + const auto lat_distance = std::abs( + (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) - + (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw)); + auto has_collision = lat_distance <= collision_distance_threshold; + if (ego_data.earliest_stop_pose) { + const auto collision_at_earliest_stop_pose = + std::abs( + (o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) - + (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)) <= + collision_distance_threshold; + has_collision |= collision_at_earliest_stop_pose; + } + return opposite_heading && has_collision; + }; + for (const auto & object : objects.objects) { + const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && - object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity && - is_in_range(object) && is_not_too_close(object)) + is_vehicle(object) && is_not_too_slow && is_in_range(object) && is_not_too_close(object) && + (!params.ignore_unavoidable_collisions || !is_unavoidable(object))) filtered_objects.push_back(object); + } return filtered_objects; } } // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp index 98b7984bab1dc..eb46a89b95ef4 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -44,7 +44,9 @@ DynamicObstacleStopModule::DynamicObstacleStopModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) { - prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type()); + prev_stop_decision_time_ = + clock->now() - + rclcpp::Duration(std::chrono::duration(params_.decision_duration_buffer)); velocity_factor_.init(PlanningBehavior::UNKNOWN); } @@ -65,10 +67,20 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); + const auto min_stop_distance = + motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, + planner_data_->max_stop_jerk_threshold) + .value_or(0.0); + ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, ego_data.pose.position, min_stop_distance); make_ego_footprint_rtree(ego_data, params_); + const auto start_time = clock_->now(); const auto has_decided_to_stop = - (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; + (start_time - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; if (!has_decided_to_stop) current_stop_pose_.reset(); double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0; const auto dynamic_obstacles = @@ -80,13 +92,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe const auto obstacle_forward_footprints = make_forward_footprints(dynamic_obstacles, params_, hysteresis); const auto footprints_duration_us = stopwatch.toc("footprints"); - const auto min_stop_distance = - motion_utils::calcDecelDistWithJerkAndAccConstraints( - planner_data_->current_velocity->twist.linear.x, 0.0, - planner_data_->current_acceleration->accel.accel.linear.x, - planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, - planner_data_->max_stop_jerk_threshold) - .value_or(0.0); stopwatch.tic("collisions"); const auto collision = find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_); @@ -101,14 +106,13 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe ? motion_utils::calcLongitudinalOffsetPose( ego_data.path.points, *collision, -params_.stop_distance_buffer - params_.ego_longitudinal_offset) - : motion_utils::calcLongitudinalOffsetPose( - ego_data.path.points, ego_data.pose.position, min_stop_distance); + : ego_data.earliest_stop_pose; if (stop_pose) { const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength( path->points, stop_pose->position, current_stop_pose_->position) > 0.0; if (use_new_stop_pose) current_stop_pose_ = *stop_pose; - prev_stop_decision_time_ = clock_->now(); + prev_stop_decision_time_ = start_time; } } diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 74fd5ca818fb0..d6961bd35a22c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -44,6 +44,7 @@ struct PlannerParam double ego_longitudinal_offset; double ego_lateral_offset; double minimum_object_distance_from_ego_path; + bool ignore_unavoidable_collisions; }; struct EgoData @@ -54,6 +55,7 @@ struct EgoData geometry_msgs::msg::Pose pose; tier4_autoware_utils::MultiPolygon2d path_footprints; Rtree rtree; + std::optional earliest_stop_pose; }; /// @brief debug data