From 36fecf2cffb96d3f942ac7c4e9c99a7e617e2083 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 25 Jul 2023 10:31:55 +0900 Subject: [PATCH] feat(dynamic_avoidance): consider a wider range of objects (#4381) * feat(dynamic_avoidance): consider a wider range of objects Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 20 +- .../dynamic_avoidance_module.hpp | 36 +- .../dynamic_avoidance_module.cpp | 377 +++++++++++------- .../dynamic_avoidance/manager.cpp | 22 + 4 files changed, 302 insertions(+), 153 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 85f60e21cf528..845f9c38e269b 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 @@ -16,21 +16,29 @@ successive_num_to_entry_dynamic_avoidance_condition: 5 - min_obj_lat_offset_to_ego_path: 0.3 # [m] + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + crossing_object: + min_object_vel: 1.0 + max_object_angle: 1.05 + + front_object: + max_object_angle: 0.785 drivable_area_generation: - lat_offset_from_obstacle: 0.8 # [m] + lat_offset_from_obstacle: 1.0 # [m] max_lat_offset_to_avoid: 0.5 # [m] # for same directional object overtaking_object: - max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 8.0 # [s] + max_time_to_collision: 10.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 3.0 # [s] + max_time_to_collision: 15.0 # [s] start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] 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 6d2b66ff025c1..fc477d485dfcc 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 @@ -48,7 +48,13 @@ struct DynamicAvoidanceParameters bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; + double min_obj_lat_offset_to_ego_path{0.0}; + double max_obj_lat_offset_to_ego_path{0.0}; + + double max_front_object_angle{0.0}; + double min_crossing_object_vel{0.0}; + double max_crossing_object_angle{0.0}; // drivable area generation double lat_offset_from_obstacle{0.0}; @@ -70,12 +76,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface struct DynamicAvoidanceObject { DynamicAvoidanceObject( - const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel) + const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, + const bool arg_is_left, const double arg_time_to_collision) : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + shape(predicted_object.shape), vel(arg_vel), lat_vel(arg_lat_vel), - shape(predicted_object.shape) + is_left(arg_is_left), + time_to_collision(arg_time_to_collision) { for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); @@ -84,12 +93,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string uuid; geometry_msgs::msg::Pose pose; + autoware_auto_perception_msgs::msg::Shape shape; double vel; double lat_vel; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector predicted_paths{}; - bool is_left; + double time_to_collision; + std::vector predicted_paths{}; }; struct DynamicAvoidanceObjectCandidate { @@ -113,7 +122,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); void updateModuleParams(const std::shared_ptr & parameters) { @@ -135,7 +144,20 @@ class DynamicAvoidanceModule : public SceneModuleInterface private: bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjectsCandidate() const; + std::vector calcTargetObjectsCandidate(); + bool willObjectCutIn( + const std::vector & ego_path, const PredictedPath & predicted_path, + const double obj_tangent_vel) const; + bool willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) 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; + std::optional> calcCollisionSection( + const std::vector & ego_path, const PredictedPath & obj_path) const; + std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; std::optional calcDynamicObstaclePolygon( 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 11198a49360fb..444515f6cd398 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 @@ -33,38 +33,6 @@ namespace behavior_path_planner { namespace { -bool isCentroidWithinLanelets( - const geometry_msgs::msg::Point & obj_pos, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - lanelet::BasicPoint2d object_centroid(obj_pos.x, obj_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - -std::vector getObjectsInLanes( - const std::vector & objects, - const lanelet::ConstLanelets & target_lanes) -{ - std::vector target_objects; - for (const auto & object : objects) { - if (isCentroidWithinLanelets(object.pose.position, target_lanes)) { - target_objects.push_back(object); - } - } - - return target_objects; -} - geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; @@ -143,6 +111,76 @@ std::pair projectObstacleVelocityToTrajectory( return std::make_pair( obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); } + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +double calcDiffAngleAgainstPath( + const std::vector & path_points, + const geometry_msgs::msg::Pose & target_pose) +{ + const size_t nearest_idx = motion_utils::findNearestIndex(path_points, target_pose.position); + const double traj_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + const double target_yaw = tf2::getYaw(target_pose.orientation); + + const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + return diff_yaw; +} + +double calcDistanceToPath( + const std::vector & path_points, + const geometry_msgs::msg::Point & target_pos) +{ + const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + if (target_idx == 0 || target_idx == path_points.size() - 1) { + const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); + const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + + if ( + (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || + (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { + return tier4_autoware_utils::calcDistance2d(path_points.at(target_idx), target_pos); + } + } + + return std::abs(motion_utils::calcLateralOffset(path_points, target_pos)); +} + +bool isLeft( + const std::vector & path_points, + const geometry_msgs::msg::Point & target_pos) +{ + const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); + const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + + if (0 < diff_yaw) { + return true; + } + return false; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( @@ -187,7 +225,6 @@ void DynamicAvoidanceModule::updateData() { // calculate target objects candidate const auto target_objects_candidate = calcTargetObjectsCandidate(); - prev_target_objects_candidate_ = target_objects_candidate; // calculate target objects considering flickering suppress target_objects_.clear(); @@ -216,13 +253,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() info_marker_.markers.clear(); debug_marker_.markers.clear(); - // 1. get reference path from previous module const auto prev_module_path = getPreviousModuleOutput().path; - - // 2. get drivable lanes from previous module const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; - // 3. create obstacles to avoid (= extract from the drivable area) + // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; prev_objects_min_bound_lat_offset_.resetCurrentUuids(); for (const auto & object : target_objects_) { @@ -241,7 +275,6 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() BehaviorModuleOutput output; output.path = prev_module_path; output.reference_path = getPreviousModuleOutput().reference_path; - // for new architecture output.drivable_area_info.drivable_lanes = drivable_lanes; output.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; @@ -293,121 +326,199 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const } std::vector -DynamicAvoidanceModule::calcTargetObjectsCandidate() const +DynamicAvoidanceModule::calcTargetObjectsCandidate() { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - // 1. convert predicted objects to dynamic avoidance objects - std::vector input_objects; + // convert predicted objects to dynamic avoidance objects + std::vector output_objects_candidate; for (const auto & predicted_object : predicted_objects) { - // check label + const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto obj_path = *std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // 1. check label const bool is_label_target_obstacle = isLabelTargetObstacle(predicted_object.classification.front().label); if (!is_label_target_obstacle) { continue; } - const auto [tangent_vel, normal_vel] = + // 2. check if velocity is large enough + const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - // check if velocity is high enough - if (std::abs(tangent_vel) < parameters_->min_obstacle_vel) { + if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { continue; } - input_objects.push_back(DynamicAvoidanceObject(predicted_object, tangent_vel, normal_vel)); - } + // 3. check if object is not crossing ego's path + const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const bool is_obstacle_crossing_path = + parameters_->max_crossing_object_angle < std::abs(obj_angle) && + parameters_->max_crossing_object_angle < M_PI - std::abs(obj_angle); + const bool is_crossing_object_to_ignore = + parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + if (is_crossing_object_to_ignore) { + continue; + } - // 2. calculate target lanes to filter obstacles - const auto [right_lanes, left_lanes] = getAdjacentLanes(100.0, 50.0); + // 4. check if object is not to be followed by ego + const double obj_dist_to_path = calcDistanceToPath(prev_module_path->points, obj_pose.position); + const bool is_object_on_ego_path = + obj_dist_to_path < + planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; + if (is_object_on_ego_path && std::abs(obj_angle) < parameters_->max_front_object_angle) { + continue; + } - // 3. filter obstacles for dynamic avoidance - const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); - const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); + // 5. check if object lateral offset to ego's path is large enough + const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); + if (is_object_far_from_path) { + continue; + } - // 4. check if object will not cut into the ego lane or cut out to the next lane, - // or close to the ego's path considering ego's lane change. - // NOTE: The oncoming object will be ignored. - constexpr double epsilon_path_lat_diff = 0.3; - std::vector output_objects_candidate; - for (const bool is_left : {true, false}) { - for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { - const auto reliable_predicted_path = std::max_element( - object.predicted_paths.begin(), object.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { - return a.confidence < b.confidence; - }); - - // Ignore object that will cut into the ego lane - const bool will_object_cut_in = [&]() { - if (object.vel < 0) { - // Ignore oncoming object - return false; - } + // 6. calculate which side object exists against ego's path + const bool is_left = isLeft(prev_module_path->points, obj_pose.position); - for (const auto & predicted_path_point : reliable_predicted_path->path) { - const double paths_lat_diff = motion_utils::calcLateralOffset( - prev_module_path->points, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } - } - return false; - }(); - if (will_object_cut_in) { - continue; - } + // 6. check if object will not cut in or cut out + const bool will_object_cut_in = + willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel); + const bool will_object_cut_out = willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_left); + if (will_object_cut_in || will_object_cut_out) { + continue; + } - // Ignore object that will cut out to the next lane - const bool will_object_cut_out = [&]() { - if (object.vel < 0) { - // Ignore oncoming object - return false; - } + // 7. check if time to collision + const double time_to_collision = + calcTimeToCollision(prev_module_path->points, obj_pose, obj_tangent_vel); + + // 8. calculate alive counter for filtering objects + const auto prev_target_object_candidate = + DynamicAvoidanceObjectCandidate::getObjectFromUuid(prev_target_objects_candidate_, obj_uuid); + const int alive_counter = + prev_target_object_candidate + ? std::min( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + prev_target_object_candidate->alive_counter + 1) + : 0; + + const auto target_object = DynamicAvoidanceObject( + predicted_object, obj_tangent_vel, obj_normal_vel, is_left, time_to_collision); + const auto target_object_candidate = + DynamicAvoidanceObjectCandidate{target_object, alive_counter}; + output_objects_candidate.push_back(target_object_candidate); + } + + prev_target_objects_candidate_ = output_objects_candidate; + return output_objects_candidate; +} - constexpr double object_lat_vel_thresh = 0.3; - if (is_left) { - if (object_lat_vel_thresh < object.lat_vel) { - return true; - } - } else { - if (object.lat_vel < -object_lat_vel_thresh) { - return true; - } +[[maybe_unused]] std::optional> +DynamicAvoidanceModule::calcCollisionSection( + const std::vector & ego_path, const PredictedPath & obj_path) const +{ + const size_t ego_idx = planner_data_->findEgoIndex(ego_path); + const double ego_vel = getEgoSpeed(); + + std::optional collision_start_idx{std::nullopt}; + double lon_dist = 0.0; + for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { + lon_dist += tier4_autoware_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + const double elapsed_time = lon_dist / ego_vel; + + const auto future_ego_pose = ego_path.at(i); + const auto future_obj_pose = + object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); + + if (future_obj_pose) { + const double dist_ego_to_obj = + tier4_autoware_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + if (dist_ego_to_obj < 1.0) { + if (!collision_start_idx) { + collision_start_idx = i; } - return false; - }(); - if (will_object_cut_out) { continue; } - - // Ignore object that is close to the ego's path. - const double lat_offset_to_path = - motion_utils::calcLateralOffset(prev_module_path->points, object.pose.position); - if ( - std::abs(lat_offset_to_path) < planner_data_->parameters.vehicle_width / 2.0 + - parameters_->min_obj_lat_offset_to_ego_path) { + } else { + if (!collision_start_idx) { continue; } + } + + return std::make_pair(*collision_start_idx, i - 1); + } + + 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 +{ + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + 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 signed_lon_length / positive_relative_velocity; +} - // get previous object if it exists - const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( - prev_target_objects_candidate_, object.uuid); - const int alive_counter = - prev_target_object_candidate - ? std::min( - parameters_->successive_num_to_entry_dynamic_avoidance_condition, - prev_target_object_candidate->alive_counter + 1) - : 0; - - auto target_object = object; - target_object.is_left = is_left; - output_objects_candidate.push_back( - DynamicAvoidanceObjectCandidate{target_object, alive_counter}); +bool DynamicAvoidanceModule::isObjectFarFromPath( + const PredictedObject & predicted_object, const double obj_dist_to_path) const +{ + const double obj_max_length = calcObstacleMaxLength(predicted_object.shape); + const double min_obj_dist_to_path = std::max( + 0.0, obj_dist_to_path - planner_data_->parameters.vehicle_width / 2.0 - obj_max_length); + + return parameters_->max_obj_lat_offset_to_ego_path < min_obj_dist_to_path; +} + +bool DynamicAvoidanceModule::willObjectCutIn( + const std::vector & ego_path, const PredictedPath & predicted_path, + const double obj_tangent_vel) const +{ + constexpr double epsilon_path_lat_diff = 0.3; + + // Ignore oncoming object + if (obj_tangent_vel < 0) { + return false; + } + + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { + return true; } } + return false; +} - return output_objects_candidate; +bool DynamicAvoidanceModule::willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const +{ + // Ignore oncoming object + if (obj_tangent_vel < 0) { + return false; + } + + constexpr double object_lat_vel_thresh = 0.3; + if (is_left) { + if (object_lat_vel_thresh < obj_normal_vel) { + return true; + } + } else { + if (obj_normal_vel < -object_lat_vel_thresh) { + return true; + } + } + return false; } std::pair DynamicAvoidanceModule::getAdjacentLanes( @@ -507,34 +618,20 @@ std::optional DynamicAvoidanceModule::calcDynam const auto [raw_min_obj_lon_offset, raw_max_obj_lon_offset] = getMinMaxValues(obj_lon_offset_vec); - - // calculate time to collision and apply it to drivable area extraction - const double relative_velocity = getEgoSpeed() - object.vel; - const double time_to_collision = [&]() { - const auto prev_module_path = getPreviousModuleOutput().path; - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position); - const double signed_lon_length = motion_utils::calcSignedArcLength( - prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx); - const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return signed_lon_length / positive_relative_velocity; - }(); - - if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + if (object.time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { return std::nullopt; } if (0 <= object.vel) { const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision); + std::min(parameters_->max_time_to_collision_overtaking_object, object.time_to_collision); return std::make_pair( raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset + object.vel * limited_time_to_collision); } const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision); + std::min(parameters_->max_time_to_collision_oncoming_object, object.time_to_collision); return std::make_pair( raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); }(); 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 cfc57e480af85..eddb93094574d 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 @@ -42,8 +42,19 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); + p.min_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "min_obj_lat_offset_to_ego_path"); + p.max_obj_lat_offset_to_ego_path = + node->declare_parameter(ns + "max_obj_lat_offset_to_ego_path"); + + p.max_front_object_angle = + node->declare_parameter(ns + "front_object.max_object_angle"); + + p.min_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_object_vel"); + p.max_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_object_angle"); } { // drivable_area_generation @@ -94,8 +105,19 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); + updateParam( parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path); + updateParam( + parameters, ns + "max_obj_lat_offset_to_ego_path", p->max_obj_lat_offset_to_ego_path); + + updateParam( + parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); + + updateParam( + parameters, ns + "crossing_object.min_object_vel", p->min_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_object_angle", p->max_crossing_object_angle); } { // drivable_area_generation