From 2f54b41ebf5a519c24e431f73c19d2a8c74911c2 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 7 Mar 2024 10:17:57 +0900 Subject: [PATCH 1/4] feat(avoidance): change lateral margin based on if it's parked vehicle (#6520) * feat(avoidance): check if it's parked vehicle Signed-off-by: satoshi-ota * feat(avoidance): change lateral margin based on if it's parked vehicle Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance_by_lane_change.param.yaml | 48 ++++++++---- .../src/manager.cpp | 10 ++- .../src/scene.cpp | 8 +- .../config/avoidance.param.yaml | 72 +++++++++++------- .../data_structs.hpp | 9 ++- .../parameter_helper.hpp | 10 ++- .../src/debug.cpp | 2 +- .../src/manager.cpp | 7 +- .../src/scene.cpp | 12 +-- .../src/shift_line_generator.cpp | 5 +- .../src/utils.cpp | 74 ++++++++++--------- 11 files changed, 155 insertions(+), 102 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index 3e8907cdccdf6..ad5fe0b123f1d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -12,64 +12,80 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] motorcycle: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] pedestrian: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 4bdcb51f1eab5..11bad993ccd3d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); return param; }; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 9f3966e055d9b..7600f604bc2fe 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -89,9 +89,11 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); const auto nearest_object_parameter = avoidance_parameters_->object_parameters.at(nearest_object_type); - const auto avoid_margin = - nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + - nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + const auto lateral_hard_margin = std::max( + nearest_object_parameter.lateral_hard_margin, + nearest_object_parameter.lateral_hard_margin_for_parked_vehicle); + const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor + + nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width; avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 4c93f7a460e42..77f91c90f7b99 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -30,81 +30,97 @@ car: execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] + moving_time_threshold: 2.0 # [s] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + envelope_buffer_margin: 0.5 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: -0.2 # [m] + hard_margin_for_parked_vehicle: -0.2 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 6aace5a939f9b..736d15cd7667c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -69,9 +69,11 @@ struct ObjectParameter double envelope_buffer_margin{0.0}; - double avoid_margin_lateral{1.0}; + double lateral_soft_margin{1.0}; - double safety_buffer_lateral{1.0}; + double lateral_hard_margin{1.0}; + + double lateral_hard_margin_for_parked_vehicle{1.0}; double safety_buffer_longitudinal{0.0}; @@ -415,6 +417,9 @@ struct ObjectData // avoidance target // is within intersection area bool is_within_intersection{false}; + // is parked vehicle on road shoulder + bool is_parked{false}; + // object direction. Direction direction{Direction::NONE}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index f1b273ae43c24..f849c1847fa38 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -72,10 +72,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); param.use_conservative_buffer_longitudinal = diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index cc7870375d0db..6f1ab41154164 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -176,7 +176,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st marker.id = uuidToInt32(object.object.object_id); marker.pose.position.z += 2.0; std::ostringstream string_stream; - string_stream << object.reason; + string_stream << object.reason << (object.is_parked ? "(PARKED)" : ""); marker.text = string_stream.str(); marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker.scale = createMarkerScale(0.6, 0.6, 0.6); diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 68cee1aa2b523..692e4260520f3 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -60,8 +60,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); - updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); - updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); + updateParam(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); + updateParam(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin); + updateParam( + parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", + config.lateral_hard_margin_for_parked_vehicle); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); updateParam( diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index cc15fe3b24bf7..49d21c15ff8a5 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -635,8 +635,8 @@ void AvoidanceModule::fillDebugData( : 0.0; const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto variable = helper_->getSharpAvoidanceDistance( helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); @@ -1408,8 +1408,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto variable = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto & additional_buffer_longitudinal = @@ -1638,8 +1638,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index cd2a08489fd29..84de6e06bdf74 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -234,9 +234,12 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto infeasible = std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER < - 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + 0.5 * data_->parameters.vehicle_width + lateral_hard_margin; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6a44350dd48c6..afe89bb3a3780 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -482,35 +482,18 @@ bool isMergingToEgoLane(const ObjectData & object) return true; } -bool isObjectOnRoadShoulder( +bool isParkedVehicle( ObjectData & object, const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { - using boost::geometry::within; using lanelet::geometry::distance2d; using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; using lanelet::utils::conversion::toLaneletPoint; - // assume that there are no parked vehicles in intersection. - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - return false; - } - - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto centerline_pos = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position).position; + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -546,7 +529,7 @@ bool isObjectOnRoadShoulder( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pose.position)).basicPoint()); + to2D(toLaneletPoint(object_pos)).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -586,7 +569,7 @@ bool isObjectOnRoadShoulder( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pose.position)).basicPoint()); + to2D(toLaneletPoint(object_pos)).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = @@ -779,7 +762,7 @@ bool isSatisfiedWithVehicleCondition( to2D(toLaneletPoint(object_pos)).basicPoint(), object.overhang_lanelet.polygon2d().basicPolygon()); if (on_ego_driving_lane) { - if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { + if (object.is_parked) { return true; } else { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; @@ -845,10 +828,13 @@ std::optional getAvoidMargin( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto min_avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; const auto soft_lateral_distance_limit = object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; const auto hard_lateral_distance_limit = @@ -1488,8 +1474,11 @@ void fillAvoidanceNecessity( { const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object_data.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto safety_margin = - 0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor; + 0.5 * vehicle_width + lateral_hard_margin * object_data.distance_factor; const auto check_necessity = [&](const auto hysteresis_factor) { return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) < @@ -1672,8 +1661,11 @@ void updateRoadShoulderDistance( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(o.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = o.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + o.avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; } const auto extract_obstacles = generateObstaclePolygonsForDrivableArea( clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0); @@ -1717,7 +1709,6 @@ void filterTargetObjects( // Find the footprint point closest to the path, set to object_data.overhang_distance. o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); - o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); // TODO(Satoshi Ota) parametrize stop time threshold if need. constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] @@ -1729,17 +1720,28 @@ void filterTargetObjects( } } - if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { - data.other_objects.push_back(o); - continue; - } - if (filtering_utils::isVehicleTypeObject(o)) { + o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { + data.other_objects.push_back(o); + continue; + } + if (!filtering_utils::isSatisfiedWithVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } else { + o.is_parked = false; + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { + data.other_objects.push_back(o); + continue; + } + if (!filtering_utils::isSatisfiedWithNonVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; @@ -2047,8 +2049,10 @@ std::pair separateObjectsByPath( double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; max_offset = std::max(max_offset, offset); } From f060da6288ffb0aef7abe171144bc292d0d9f5a3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 7 Mar 2024 20:45:49 +0900 Subject: [PATCH 2/4] fix(drivable_area_expansion): fix invalid access (#6566) Signed-off-by: satoshi-ota --- .../utils/drivable_area_expansion/static_drivable_area.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 8c1fae40712ef..41fd49687302a 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1482,6 +1482,10 @@ std::vector postProcess( const auto & vehicle_length = planner_data->parameters.vehicle_length; constexpr double overlap_threshold = 0.01; + if (original_bound.size() < 2) { + return original_bound; + } + const auto addPoints = [](const lanelet::ConstLineString3d & points, std::vector & bound) { for (const auto & bound_p : points) { From dc15ad5fae72903d0c7c13a359cf1c391868c623 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 12 Mar 2024 09:53:17 +0900 Subject: [PATCH 3/4] fix(avoidance): ignore objects are in intersection when the ego stops by red traffic signal (#6565) fix(avoidance): don't avoid objects are farther than red signal traffic light Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 49d21c15ff8a5..f0dda0022f8fd 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -321,10 +321,19 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; + using utils::traffic_light::calcDistanceToRedTrafficLight; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. constexpr double MARGIN = 10.0; - const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto forward_detection_range = [&]() { + const auto to_traffic_light = calcDistanceToRedTrafficLight( + data.current_lanelets, helper_->getPreviousReferencePath(), planner_data_); + if (!to_traffic_light.has_value()) { + return helper_->getForwardDetectionRange(); + } + return std::min(helper_->getForwardDetectionRange(), to_traffic_light.value()); + }(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, data, parameters_, forward_detection_range + MARGIN, debug); From 2bccaa11a07bd8b46f5b1d23a5de1f1a277354b1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 15 Mar 2024 11:01:06 +0900 Subject: [PATCH 4/4] fix(avoidance): the module ignored merging objects unexpectedly (#6601) * feat(avoidance): output overhang lanelet Signed-off-by: satoshi-ota * fix(avoidance): avoid merging vehicle if it's NOT on ego lane. Signed-off-by: satoshi-ota * fix(avoidance): add flag to identify ambiguous vehicle Signed-off-by: satoshi-ota * refactor(avoidance): add helper function Signed-off-by: satoshi-ota * refactor(avoidance): rename param Signed-off-by: satoshi-ota * chore(avoidance): update comment Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 2 +- .../config/avoidance.param.yaml | 2 +- .../data_structs.hpp | 9 ++++- .../behavior_path_avoidance_module/helper.hpp | 19 +++++++++ .../parameter_helper.hpp | 2 +- .../src/debug.cpp | 25 +++++++++++- .../src/scene.cpp | 29 ++++++-------- .../src/shift_line_generator.cpp | 29 +++++--------- .../src/utils.cpp | 39 +++++++++++-------- 9 files changed, 96 insertions(+), 60 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 11bad993ccd3d..19e03181e1dcb 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -132,7 +132,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) } { - const std::string ns = "avoidance.target_filtering.force_avoidance."; + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; p.enable_force_avoidance_for_stopped_vehicle = getOrDeclareParameter(*node, ns + "enable"); p.threshold_time_force_avoidance_for_stopped_vehicle = diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 77f91c90f7b99..fa7cf2d11c56d 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -156,7 +156,7 @@ backward_distance: 10.0 # [m] # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. - force_avoidance: + avoidance_for_ambiguous_vehicle: enable: false # [-] time_threshold: 1.0 # [s] distance_threshold: 1.0 # [m] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 736d15cd7667c..325a190be0412 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -384,7 +384,8 @@ struct ObjectData // avoidance target rclcpp::Time last_move; double stop_time{0.0}; - // store the information of the lanelet which the object's overhang is currently occupying + // It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to + // check whether the object is on the ego lane. lanelet::ConstLanelet overhang_lanelet; // the position at the detected moment @@ -420,6 +421,12 @@ struct ObjectData // avoidance target // is parked vehicle on road shoulder bool is_parked{false}; + // is driving on ego current lane + bool is_on_ego_lane{false}; + + // is ambiguous stopped vehicle. + bool is_ambiguous{false}; + // object direction. Direction direction{Direction::NONE}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 4f15261f42246..5ade0dd0062b0 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -17,6 +17,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include @@ -134,6 +135,24 @@ class AvoidanceHelper shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } + double getFrontConstantDistance(const ObjectData & object) const + { + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front + : 0.0; + return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + } + + double getRearConstantDistance(const ObjectData & object) const + { + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear + + object.length; + } + double getEgoShift() const { validate(); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index f849c1847fa38..638d3c30d8058 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -140,7 +140,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) } { - const std::string ns = "avoidance.target_filtering.force_avoidance."; + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; p.enable_force_avoidance_for_stopped_vehicle = getOrDeclareParameter(*node, ns + "enable"); p.threshold_time_force_avoidance_for_stopped_vehicle = diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 6f1ab41154164..3c80a62a4fe2c 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -188,10 +188,25 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st return msg; } +MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size()); + + for (const auto & object : objects) { + appendMarkerArray( + marker_utils::createLaneletsAreaMarkerArray( + {object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0), + &msg); + } + + return msg; +} + MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - msg.markers.reserve(objects.size() * 4); + msg.markers.reserve(objects.size() * 5); appendMarkerArray( createObjectsCubeMarkerArray( @@ -202,6 +217,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -209,7 +225,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - msg.markers.reserve(objects.size() * 4); + msg.markers.reserve(objects.size() * 5); appendMarkerArray( createObjectsCubeMarkerArray( @@ -220,6 +236,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -451,6 +468,10 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const appendMarkerArray( createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"), &msg); + appendMarkerArray( + createOverhangLaneletMarkerArray( + filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"), + &msg); return msg; } diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index f0dda0022f8fd..cddb18f988719 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -638,21 +638,16 @@ void AvoidanceModule::fillDebugData( const auto o_front = data.stop_target_object.value(); const auto object_type = utils::getHighestProbLabel(o_front.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; + const auto constant_distance = helper_->getFrontConstantDistance(o_front); const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto variable = helper_->getSharpAvoidanceDistance( + const auto avoidance_distance = helper_->getSharpAvoidanceDistance( helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); - const auto constant = helper_->getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal; - const auto total_avoid_distance = variable + constant; + const auto prepare_distance = helper_->getNominalPrepareDistance(); + const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; dead_pose_ = calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); @@ -1419,16 +1414,14 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto variable = helper_->getMinAvoidanceDistance( + const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + p->stop_buffer; - - return object.longitudinal - std::min(variable + constant, p->stop_max_distance); + const auto constant_distance = helper_->getFrontConstantDistance(object); + + return object.longitudinal - + std::min( + avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer, + p->stop_max_distance); } void AvoidanceModule::insertReturnDeadLine( diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 84de6e06bdf74..5a8fd35f6d618 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -123,7 +123,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( { // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = helper_->getEgoShift(); - const auto & base_link2rear = data_->parameters.base_link2rear; // Calculate feasible shift length const auto get_shift_profile = @@ -140,13 +139,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate remaining distance. const auto prepare_distance = helper_->getNominalPrepareDistance(); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - const auto constant = object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + prepare_distance; - const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; - const auto remaining_distance = object.longitudinal - constant; + const auto constant_distance = helper_->getFrontConstantDistance(object); + const auto has_enough_distance = + object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance; const auto avoidance_distance = has_enough_distance ? nominal_avoid_distance : remaining_distance; @@ -278,11 +274,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } } - // use each object param - const auto object_type = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; @@ -297,12 +290,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidLine al_avoid; { - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - const auto offset = - object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; - const auto to_shift_end = o.longitudinal - offset; + const auto constant_distance = helper_->getFrontConstantDistance(o); + const auto to_shift_end = o.longitudinal - constant_distance; const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index); // start point (use previous linear shift length as start shift length.) @@ -338,8 +327,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidLine al_return; { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - const auto to_shift_start = o.longitudinal + offset; + const auto constant_distance = helper_->getRearConstantDistance(o); + const auto to_shift_start = o.longitudinal + constant_distance; // start point al_return.start_shift_length = feasible_shift_profile.value().first; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index afe89bb3a3780..6338e035c93dd 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -444,6 +444,14 @@ bool isWithinIntersection( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } +bool isOnEgoLane(const ObjectData & object) +{ + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon()); +} + bool isParallelToEgoLane(const ObjectData & object, const double threshold) { const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; @@ -491,6 +499,10 @@ bool isParkedVehicle( using lanelet::utils::to2D; using lanelet::utils::conversion::toLaneletPoint; + if (object.is_within_intersection) { + return false; + } + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto centerline_pos = lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; @@ -579,15 +591,11 @@ bool isParkedVehicle( return is_left_side_parked_vehicle || is_right_side_parked_vehicle; } -bool isForceAvoidanceTarget( +bool isAmbiguousStoppedVehicle( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - if (!parameters->enable_force_avoidance_for_stopped_vehicle) { - return false; - } - const auto stop_time_longer_than_threshold = object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; @@ -618,6 +626,10 @@ bool isForceAvoidanceTarget( return false; } + if (!object.is_on_ego_lane) { + return true; + } + const auto & ego_pose = planner_data->self_odometry->pose.pose; // force avoidance for stopped vehicle @@ -744,24 +756,17 @@ bool isSatisfiedWithVehicleCondition( const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - using boost::geometry::within; - using lanelet::utils::to2D; - using lanelet::utils::conversion::toLaneletPoint; - object.behavior = getObjectBehavior(object, parameters); - object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); + object.is_on_ego_lane = isOnEgoLane(object); + object.is_ambiguous = isAmbiguousStoppedVehicle(object, data, planner_data, parameters); // from here condition check for vehicle type objects. - if (isForceAvoidanceTarget(object, data, planner_data, parameters)) { + if (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) { return true; } // check vehicle shift ratio - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - const auto on_ego_driving_lane = within( - to2D(toLaneletPoint(object_pos)).basicPoint(), - object.overhang_lanelet.polygon2d().basicPolygon()); - if (on_ego_driving_lane) { + if (object.is_on_ego_lane) { if (object.is_parked) { return true; } else { @@ -1721,6 +1726,8 @@ void filterTargetObjects( } if (filtering_utils::isVehicleTypeObject(o)) { + o.is_within_intersection = + filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);