From 98c01da55c6e715157668b71c96c189bf55fe48e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Dec 2023 14:43:47 +0900 Subject: [PATCH] =?UTF-8?q?feat(dynamic=5Favoidance):=20change=20the=20log?= =?UTF-8?q?ic=20of=20longitudinal=20distance=20to=E2=80=A6=20(#5947)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(dynamic_avoidance): change the logic of longitudinal distance to avoid Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 3 + .../dynamic_avoidance_module.hpp | 18 +- .../dynamic_avoidance_module.cpp | 251 +++++++++++++----- .../dynamic_avoidance/manager.cpp | 6 + 4 files changed, 202 insertions(+), 76 deletions(-) diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 854c29aa89bc5..29fdca78e5bcb 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -46,6 +46,9 @@ min_object_vel: -0.5 # [m/s] The value is negative considering the noisy velocity of the stopped vehicle. max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value. + stopped_object: + max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value. + drivable_area_generation: polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base" object_path_base: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 36106f32158b9..3ceff4244f2c8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -87,6 +87,7 @@ struct DynamicAvoidanceParameters double max_overtaking_crossing_object_angle{0.0}; double min_oncoming_crossing_object_vel{0.0}; double max_oncoming_crossing_object_angle{0.0}; + double max_stopped_object_vel{0.0}; // drivable area generation PolygonGenerationMethod polygon_generation_method{}; @@ -106,6 +107,12 @@ struct DynamicAvoidanceParameters double end_duration_to_avoid_oncoming_object{0.0}; }; +struct TimeWhileCollision +{ + double time_to_start_collision; + double time_to_end_collision; +}; + class DynamicAvoidanceModule : public SceneModuleInterface { public: @@ -324,9 +331,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; - double calcTimeToCollision( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + TimeWhileCollision calcTimeWhileCollision( + const std::vector & ego_path, const double obj_tangent_vel, + const LatLonOffset & lat_lon_offset) const; std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( @@ -334,12 +341,13 @@ class DynamicAvoidanceModule : public SceneModuleInterface const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const bool is_object_same_direction) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const double time_to_collision) const; + const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 8772a1543b20b..87a940d04e8d3 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -570,25 +570,30 @@ void DynamicAvoidanceModule::updateTargetObjects() // } // 2.e. check time to collision - const double time_to_collision = - calcTimeToCollision(input_path.points, object.pose, object.vel, lat_lon_offset); - if ( - (0 <= object.vel && - parameters_->max_time_to_collision_overtaking_object < time_to_collision) || - (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", - obj_uuid.c_str(), time_to_collision); - continue; - } - if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative " - "value.", - obj_uuid.c_str(), time_to_collision); - continue; + const auto time_while_collision = + calcTimeWhileCollision(input_path.points, object.vel, lat_lon_offset); + const double time_to_collision = time_while_collision.time_to_start_collision; + if (parameters_->max_stopped_object_vel < std::hypot(object.vel, object.lat_vel)) { + // NOTE: Only not stopped object is filtered by time to collision. + if ( + (0 <= object.vel && + parameters_->max_time_to_collision_overtaking_object < time_to_collision) || + (object.vel <= 0 && + parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", + obj_uuid.c_str(), time_to_collision); + continue; + } + if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small " + "negative value.", + obj_uuid.c_str(), time_to_collision); + continue; + } } // 2.f. calculate which side object will be against ego's path @@ -630,7 +635,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, - time_to_collision); + time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, prev_object); @@ -716,26 +721,60 @@ DynamicAvoidanceModule::calcCollisionSection( return std::make_pair(*collision_start_idx, ego_path.size() - 1); } -double DynamicAvoidanceModule::calcTimeToCollision( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const +TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( + const std::vector & ego_path, const double obj_tangent_vel, + const LatLonOffset & lat_lon_offset) const { // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); - const double lon_offset_ego_to_obj = - motion_utils::calcSignedArcLength( - ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + - lat_lon_offset.max_lon_offset; - const double maximum_time_to_collision = - (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits::max(); - + const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; - const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); - const double signed_lon_length = motion_utils::calcSignedArcLength( - ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); - const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); + + const double signed_time_to_start_collision = [&]() { + const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx + + lat_lon_offset.min_lon_offset - + planner_data_->parameters.front_overhang; + const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx - + lat_lon_offset.max_lon_offset - + planner_data_->parameters.rear_overhang; + if (0.0 < lon_offset_ego_front_to_obj_back) { // The object is ahead of the ego. + return lon_offset_ego_front_to_obj_back / relative_velocity; + } else if (0.0 < lon_offset_obj_front_to_ego_back) { // The ego is ahead of the object. + return lon_offset_obj_front_to_ego_back / -relative_velocity; + } + // The ego and object are colliding. + return 0.0; + }(); + const double signed_time_to_end_collision = [&]() { + const double lon_offset_ego_back_to_obj_front = lon_offset_ego_to_obj_idx + + lat_lon_offset.max_lon_offset + + planner_data_->parameters.rear_overhang; + const double lon_offset_obj_back_to_ego_front = -lon_offset_ego_to_obj_idx - + lat_lon_offset.min_lon_offset + + planner_data_->parameters.front_overhang; + if (0.0 < relative_velocity) { + return lon_offset_ego_back_to_obj_front / relative_velocity; + } + return lon_offset_obj_back_to_ego_front / -relative_velocity; + }(); + + // NOTE: In order to make time_to_start_collision continuous around the relative_velocity is zero. + const double time_to_start_collision = [&]() { + if (signed_time_to_start_collision < 0.0) { + return std::numeric_limits::max(); + } + return signed_time_to_start_collision; + }(); + const double time_to_end_collision = [&]() { + if (signed_time_to_end_collision < 0.0) { + return std::numeric_limits::max(); + } + return signed_time_to_end_collision; + }(); + + return TimeWhileCollision{time_to_start_collision, time_to_end_collision}; } bool DynamicAvoidanceModule::isObjectFarFromPath( @@ -928,7 +967,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const double time_to_collision) const + const TimeWhileCollision & time_while_collision) const { const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); @@ -943,40 +982,54 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( obj_lon_offset_vec.push_back(lon_offset); } - const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec); + return getMinMaxValues(obj_lon_offset_vec); + }(); - if (obj_vel < 0) { - return MinMaxValue{ - raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; - } + const double relative_velocity = getEgoSpeed() - obj_vel; - // NOTE: If time to collision is considered here, the ego is close to the object when starting - // avoidance. - // The ego should start avoidance before overtaking. - return raw_obj_lon_offset; + // calculate bound start and end length + const double start_length_to_avoid = [&]() { + if (obj_vel < parameters_->max_stopped_object_vel) { + // The ego and object are the same directional or the object is parked. + return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; + } + // The ego and object are the opposite directional. + const double obj_acc = -1.0; + const double decel_time = 1.0; + const double obj_moving_dist = + (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / + obj_acc; + const double ego_moving_dist = getEgoSpeed() * decel_time; + return std::max(0.0, ego_moving_dist - obj_moving_dist) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; + }(); + const double end_length_to_avoid = [&]() { + if (obj_vel < parameters_->max_stopped_object_vel) { + // The ego and object are the same directional or the object is parked. + return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object; + } + // The ego and object are the opposite directional. + return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + + std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; }(); - // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= obj_vel); - // TODO(murooka) use getEgoSpeed() instead of obj_vel - const double start_length_to_avoid = - std::abs(obj_vel) * (is_object_overtaking - ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); - const double end_length_to_avoid = - std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); - - const double valid_length_to_avoid = calcValidLengthToAvoid(obj_path, obj_pose, obj_shape); - if (obj_vel < -0.5) { + // calculate valid path for the forked object's path from the ego's path + if (obj_vel < -parameters_->max_stopped_object_vel) { + const bool is_object_same_direction = false; + const double valid_length_to_avoid = + calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction); return MinMaxValue{ - std::max(obj_lon_offset.min_value - start_length_to_avoid, -valid_length_to_avoid), + obj_lon_offset.min_value + std::max(-start_length_to_avoid, -valid_length_to_avoid), obj_lon_offset.max_value + end_length_to_avoid}; } - if (0.5 < obj_vel) { + if (parameters_->max_stopped_object_vel < obj_vel) { + const bool is_object_same_direction = true; + const double valid_length_to_avoid = + calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction); return MinMaxValue{ obj_lon_offset.min_value - start_length_to_avoid, - std::min(obj_lon_offset.max_value + end_length_to_avoid, valid_length_to_avoid)}; + obj_lon_offset.max_value + std::min(end_length_to_avoid, valid_length_to_avoid)}; } return MinMaxValue{ obj_lon_offset.min_value - start_length_to_avoid, @@ -985,25 +1038,44 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( double DynamicAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const + const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const bool is_object_same_direction) const { - const auto input_path_points = getPreviousModuleOutput().path.points; + const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); + const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 + + parameters_->lat_offset_from_obstacle + + calcObstacleMaxLength(obj_shape); + + // crop the ego's path by object position + std::vector cropped_ego_path_points; + if (is_object_same_direction) { + cropped_ego_path_points = std::vector{ + input_path_points.begin() + obj_seg_idx, input_path_points.end()}; + } else { + cropped_ego_path_points = std::vector{ + input_path_points.begin(), input_path_points.begin() + obj_seg_idx + 1 + 1}; + std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end()); + } + if (cropped_ego_path_points.size() < 2) { + return motion_utils::calcArcLength(obj_path.path); + } + + // calculate where the object's path will be forked from (= far from) the ego's path. + std::optional last_nearest_ego_path_seg_idx{std::nullopt}; const size_t valid_obj_path_end_idx = [&]() { - int ego_path_idx = obj_seg_idx + 1; + size_t ego_path_seg_idx = 0; for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) { bool are_paths_close{false}; - for (; 0 < ego_path_idx; --ego_path_idx) { + for (; ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) { const double dist_to_segment = calcDistanceToSegment( obj_path.path.at(obj_path_idx).position, - input_path_points.at(ego_path_idx).point.pose.position, - input_path_points.at(ego_path_idx - 1).point.pose.position); - if ( - dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 + - parameters_->lat_offset_from_obstacle + - calcObstacleMaxLength(obj_shape)) { + cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position, + cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position); + if (dist_to_segment < dist_threshold_paths) { + last_nearest_ego_path_seg_idx = ego_path_seg_idx; are_paths_close = true; break; } @@ -1015,6 +1087,43 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( } return obj_path.path.size() - 1; }(); + + // calculate valid length to avoid + if (last_nearest_ego_path_seg_idx && valid_obj_path_end_idx != obj_path.path.size() - 1) { + const auto calc_min_dist = [&](const size_t arg_obj_path_idx) -> std::optional { + std::optional min_dist{std::nullopt}; + for (size_t ego_path_seg_idx = *last_nearest_ego_path_seg_idx; + ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) { + const double dist_to_segment = calcDistanceToSegment( + obj_path.path.at(arg_obj_path_idx).position, + cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position, + cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position); + if (!min_dist || dist_to_segment < *min_dist) { + min_dist = dist_to_segment; + } + if (min_dist && *min_dist < dist_to_segment) { + return *min_dist; + } + } + return min_dist; + }; + const size_t prev_valid_obj_path_end_idx = + (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx : valid_obj_path_end_idx - 1; + const size_t next_valid_obj_path_end_idx = + (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx + 1 : valid_obj_path_end_idx; + const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); + const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); + if (prev_min_dist && next_min_dist) { + const double segment_length = tier4_autoware_utils::calcDistance2d( + obj_path.path.at(prev_valid_obj_path_end_idx), + obj_path.path.at(next_valid_obj_path_end_idx)); + const double partial_segment_length = segment_length * + (dist_threshold_paths - *prev_min_dist) / + (*next_min_dist - *prev_min_dist); + return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) + + partial_segment_length; + } + } return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index ca0bc070d0fdb..73d263319c2d9 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -99,6 +99,9 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel"); p.max_oncoming_crossing_object_angle = node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle"); + + p.max_stopped_object_vel = + node->declare_parameter(ns + "stopped_object.max_object_vel"); } { // drivable_area_generation @@ -207,6 +210,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "crossing_object.max_oncoming_object_angle", p->max_oncoming_crossing_object_angle); + + updateParam( + parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel); } { // drivable_area_generation