From 8de40bbdc2b8a941525c7397ef0c8ca42f2f0924 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Dec 2023 00:40:52 +0900 Subject: [PATCH 01/48] fix(avoidance): update logic to calculate centerline distance (#5855) * fix(avoidance): use centerline of current lanelets in object filtering logic Signed-off-by: satoshi-ota * fix(avoidance): fix test Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../package.xml | 1 + .../src/scene.cpp | 9 +++++++-- .../behavior_path_avoidance_module/data_structs.hpp | 11 +++++++---- planning/behavior_path_avoidance_module/src/debug.cpp | 11 +++++++++-- planning/behavior_path_avoidance_module/src/scene.cpp | 10 ++++++---- planning/behavior_path_avoidance_module/src/utils.cpp | 11 +++++++---- .../test/test_utils.cpp | 8 ++------ 7 files changed, 39 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index 3a4f42f2d2a6c..0f9f3f6dc9d42 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -22,6 +22,7 @@ behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common + lanelet2_extension motion_utils pluginlib rclcpp 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 619cbc515f816..e245f1c44fe0e 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 @@ -20,6 +20,7 @@ #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include @@ -232,6 +233,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( using boost::geometry::return_centroid; using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcLateralDeviation; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -263,8 +265,11 @@ ObjectData AvoidanceByLaneChange::createObjectData( utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); // Calc lateral deviation from path to target object. - object_data.lateral = - tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + object_data.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( 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 8e294e9e3237e..ad38b511bb89b 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 @@ -52,6 +52,8 @@ using geometry_msgs::msg::TransformStamped; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using route_handler::Direction; + struct ObjectParameter { bool is_avoidance_target{false}; @@ -327,14 +329,14 @@ struct ObjectData // avoidance target { ObjectData() = default; ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) - : object(obj), lateral(lat), longitudinal(lon), length(len), overhang_dist(overhang) + : object(obj), to_centerline(lat), longitudinal(lon), length(len), overhang_dist(overhang) { } PredictedObject object; // lateral position of the CoM, in Frenet coordinate from ego-pose - double lateral; + double to_centerline; // longitudinal position of the CoM, in Frenet coordinate from ego-pose double longitudinal; @@ -396,6 +398,9 @@ struct ObjectData // avoidance target // is within intersection area bool is_within_intersection{false}; + // object direction. + Direction direction{Direction::NONE}; + // unavoidable reason std::string reason{""}; @@ -566,8 +571,6 @@ struct ShiftLineData */ struct DebugData { - std::shared_ptr current_lanelets; - geometry_msgs::msg::Polygon detection_area; lanelet::ConstLineStrings3d bounds; diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 310613ee19678..47a4f3228819c 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include @@ -160,7 +161,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral:" << object.lateral << " [m]\n" + << "lateral:" << object.to_centerline << " [m]\n" << "necessity:" << object.avoid_required << " [-]\n" << "stoppable:" << object.is_stoppable << " [-]\n" << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" @@ -495,6 +496,8 @@ MarkerArray createDrivableBounds( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) { + using behavior_path_planner::utils::transformToLanelets; + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; @@ -608,9 +611,13 @@ MarkerArray createDebugMarkerArray( // misc { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + add(laneletsAsTriangleMarkerArray( + "drivable_lanes", transformToLanelets(data.drivable_lanes), + createMarkerColor(0.16, 1.0, 0.69, 0.2))); + add(laneletsAsTriangleMarkerArray( + "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2))); } return msg; diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index ebe42f73d538b..979daa3118ad4 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -328,14 +328,12 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // debug { - debug.current_lanelets = std::make_shared(data.current_lanelets); - std::vector debug_info_array; const auto append = [&](const auto & o) { AvoidanceDebugMsg debug_info; debug_info.object_id = toHexString(o.object.object_id); debug_info.longitudinal_distance = o.longitudinal; - debug_info.lateral_distance_from_centerline = o.lateral; + debug_info.lateral_distance_from_centerline = o.to_centerline; debug_info.allow_avoidance = o.reason == ""; debug_info.failed_reason = o.reason; debug_info_array.push_back(debug_info); @@ -380,7 +378,11 @@ ObjectData AvoidanceModule::createObjectData( utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); + object_data.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index fd120bdd8a069..d6c7c02a5d81b 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -594,7 +594,7 @@ bool isSatisfiedWithVehicleCondition( } // Object is on center line -> ignore. - if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) { + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; return false; } @@ -678,7 +678,11 @@ std::optional getAvoidMargin( bool isOnRight(const ObjectData & obj) { - return obj.lateral < 0.0; + if (obj.direction == Direction::NONE) { + throw std::logic_error("object direction is not initialized. something wrong."); + } + + return obj.direction == Direction::RIGHT; } double calcShiftLength( @@ -960,9 +964,8 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - const bool is_left = 0 < object.lateral; obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, is_left}); + {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; } diff --git a/planning/behavior_path_avoidance_module/test/test_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp index e5c6fd2e72092..95488daa69cf8 100644 --- a/planning/behavior_path_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -25,7 +25,7 @@ using behavior_path_planner::utils::avoidance::isSameDirectionShift; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { ObjectData right_obj; - right_obj.lateral = -0.3; + right_obj.direction = route_handler::Direction::RIGHT; const double negative_shift_length = -1.0; const double positive_shift_length = 1.0; @@ -33,11 +33,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), positive_shift_length)); ObjectData left_obj; - left_obj.lateral = 0.3; + left_obj.direction = route_handler::Direction::LEFT; ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), positive_shift_length)); ASSERT_FALSE(isSameDirectionShift(isOnRight(left_obj), negative_shift_length)); - - const double zero_shift_length = 0.0; - ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), zero_shift_length)); - ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), zero_shift_length)); } From 9d8040b899cefa375a3e404ffb67acdf254d42e4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Dec 2023 03:16:06 +0900 Subject: [PATCH 02/48] fix(avoidance): check moving distance (#5922) Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 1 + .../data_structs.hpp | 4 +++ .../parameter_helper.hpp | 2 ++ .../behavior_path_avoidance_module/scene.hpp | 8 +++++- .../behavior_path_avoidance_module/utils.hpp | 2 ++ .../src/scene.cpp | 3 +++ .../src/utils.cpp | 26 ++++++++++++++++++- 7 files changed, 44 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index c68c10f2a9489..2d38cebd591f5 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -141,6 +141,7 @@ force_avoidance: enable: false # [-] time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.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 ad38b511bb89b..ef5d090073307 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 @@ -177,6 +177,7 @@ struct AvoidanceParameters // force avoidance double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; + double force_avoidance_distance_threshold{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction @@ -368,6 +369,9 @@ struct ObjectData // avoidance target // store the information of the lanelet which the object's overhang is currently occupying lanelet::ConstLanelet overhang_lanelet; + // the position at the detected moment + Pose init_pose; + // the position of the overhang Pose overhang_pose; 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 212ed04ade6c6..eb5cb18ce697c 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,6 +140,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "enable"); p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter(*node, ns + "time_threshold"); + p.force_avoidance_distance_threshold = + getOrDeclareParameter(*node, ns + "distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 882806177b4e0..473b1d18c59ef 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -433,14 +433,20 @@ class AvoidanceModule : public SceneModuleInterface UUID candidate_uuid_; + // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; - mutable size_t safe_count_{0}; + // TODO(Satoshi OTA) remove mutable. + mutable ObjectDataArray detected_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray ego_stopped_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray stopped_objects_; + mutable size_t safe_count_{0}; + mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 521058e3a43c9..9ce331e70d8b2 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -117,6 +117,8 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects); + void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 979daa3118ad4..952747cd64791 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -377,6 +377,9 @@ ObjectData AvoidanceModule::createObjectData( // Calc moving time. utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); + // Fill init pose. + utils::avoidance::fillInitialPose(object_data, detected_objects_); + // Calc lateral deviation from path to target object. object_data.to_centerline = lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index d6c7c02a5d81b..64f08916461a9 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -463,6 +463,15 @@ bool isForceAvoidanceTarget( return false; } + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->force_avoidance_distance_threshold; + + if (is_moving_distance_longer_than_threshold) { + return false; + } + if (object.is_within_intersection) { RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); return false; @@ -478,7 +487,6 @@ bool isForceAvoidanceTarget( } const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle bool not_parked_object = true; @@ -1235,6 +1243,22 @@ void fillAvoidanceNecessity( object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects) +{ + const auto id = object_data.object.object_id; + const auto same_id_obj = std::find_if( + detected_objects.begin(), detected_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + if (same_id_obj != detected_objects.end()) { + object_data.init_pose = same_id_obj->init_pose; + return; + } + + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + detected_objects.push_back(object_data); +} + void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) From be82c0929bd52cbcccb60fc82eb06c316939df74 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Dec 2023 15:48:34 +0900 Subject: [PATCH 03/48] feat(avoidance): don't avoid merging vehicle (#5720) * feat(avoidance): add new filter for objects in intersection Signed-off-by: satoshi-ota * fix(avoidance): fix object filtering Signed-off-by: satoshi-ota * fix(avoidance): avoid simple merging object Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../data_structs.hpp | 8 +++ .../src/debug.cpp | 2 + .../src/utils.cpp | 53 ++++++++++++++++++- 3 files changed, 62 insertions(+), 1 deletion(-) 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 ef5d090073307..4b88eb669a2cf 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 @@ -336,6 +336,14 @@ struct ObjectData // avoidance target PredictedObject object; + // object behavior. + enum class Behavior { + NONE = 0, + MERGING, + DEVIATING, + }; + Behavior behavior{Behavior::NONE}; + // lateral position of the CoM, in Frenet coordinate from ego-pose double to_centerline; diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 47a4f3228819c..9d0492829c53b 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -557,6 +557,8 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); addObjects(data.other_objects, std::string("TooNearToGoal")); + addObjects(data.other_objects, std::string("ParallelToEgoLane")); + addObjects(data.other_objects, std::string("MergingToEgoLane")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 64f08916461a9..f28ad324267cc 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -333,6 +333,34 @@ bool isParallelToEgoLane(const ObjectData & object, const double threshold) return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } +bool isMergingToEgoLane(const ObjectData & object) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + + if (isOnRight(object)) { + if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { + return false; + } + + if (yaw_deviation > M_PI_2) { + return false; + } + } else { + if (yaw_deviation > 0.0 && M_PI_2 > yaw_deviation) { + return false; + } + + if (yaw_deviation < -1.0 * M_PI_2) { + return false; + } + } + + return true; +} + bool isObjectOnRoadShoulder( ObjectData & object, const std::shared_ptr & route_handler, const std::shared_ptr & parameters) @@ -587,6 +615,17 @@ bool isSatisfiedWithNonVehicleCondition( return true; } +ObjectData::Behavior getObjectBehavior( + ObjectData & object, const std::shared_ptr & parameters) +{ + if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + return ObjectData::Behavior::NONE; + } + + return isMergingToEgoLane(object) ? ObjectData::Behavior::MERGING + : ObjectData::Behavior::DEVIATING; +} + bool isSatisfiedWithVehicleCondition( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, @@ -594,6 +633,7 @@ bool isSatisfiedWithVehicleCondition( { using boost::geometry::within; + object.behavior = getObjectBehavior(object, parameters); object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); // from here condition check for vehicle type objects. @@ -601,6 +641,12 @@ bool isSatisfiedWithVehicleCondition( return true; } + // always ignore all merging objects. + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + return false; + } + // Object is on center line -> ignore. if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; @@ -623,7 +669,12 @@ bool isSatisfiedWithVehicleCondition( return true; } - if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "straight") { + return true; + } + + if (object.behavior == ObjectData::Behavior::NONE) { object.reason = "ParallelToEgoLane"; return false; } From 1c93dcc76e26d01624a4cb3719e689f5773925fb Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 21 Dec 2023 16:52:33 +0900 Subject: [PATCH 04/48] refactor(avoidance): remove unused header (#5907) * refactor(avoidance): remove unused header Signed-off-by: Muhammad Zulfaqar Azmi * remove libboost-dev dependencies Signed-off-by: Muhammad Zulfaqar Azmi * removed unused variable. add slight clang-tidy suggestion Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../data_structs.hpp | 29 +++++++++++-------- .../behavior_path_avoidance_module/debug.hpp | 16 ++-------- .../behavior_path_avoidance_module/helper.hpp | 2 +- .../manager.hpp | 5 ++-- .../behavior_path_avoidance_module/scene.hpp | 15 ++++------ .../shift_line_generator.hpp | 5 ---- .../package.xml | 1 - .../src/debug.cpp | 4 +-- .../src/scene.cpp | 13 ++++----- .../src/shift_line_generator.cpp | 2 +- .../src/utils.cpp | 14 +++------ ...t_behavior_path_planner_node_interface.cpp | 3 -- 12 files changed, 40 insertions(+), 69 deletions(-) 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 4b88eb669a2cf..be8bcb3fb970d 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 @@ -19,12 +19,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include #include -#include #include #include @@ -34,6 +33,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -48,7 +48,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::TransformStamped; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; @@ -329,8 +328,13 @@ struct AvoidanceParameters struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) - : object(obj), to_centerline(lat), longitudinal(lon), length(len), overhang_dist(overhang) + + ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang) + : object(std::move(obj)), + to_centerline(lat), + longitudinal(lon), + length(len), + overhang_dist(overhang) { } @@ -345,16 +349,17 @@ struct ObjectData // avoidance target Behavior behavior{Behavior::NONE}; // lateral position of the CoM, in Frenet coordinate from ego-pose - double to_centerline; + + double to_centerline{0.0}; // longitudinal position of the CoM, in Frenet coordinate from ego-pose - double longitudinal; + double longitudinal{0.0}; // longitudinal length of vehicle, in Frenet coordinate - double length; + double length{0.0}; // lateral distance to the closest footprint, in Frenet coordinate - double overhang_dist; + double overhang_dist{0.0}; // lateral shiftable ratio double shiftable_ratio{0.0}; @@ -414,7 +419,7 @@ struct ObjectData // avoidance target Direction direction{Direction::NONE}; // unavoidable reason - std::string reason{""}; + std::string reason{}; // lateral avoid margin std::optional avoid_margin{std::nullopt}; @@ -457,8 +462,8 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) - : avoid_line{avoid_line}, return_line{return_line} + AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 2705af5af4020..f49f457ddd066 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -16,33 +16,23 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include -#include #include -#include #include -#include - #include -#include namespace marker_utils::avoidance_marker { -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; -using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; @@ -50,7 +40,7 @@ MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); MarkerArray createAvoidLineMarkerArray( - const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, + const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w); MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); @@ -63,11 +53,11 @@ MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr); +std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr); -std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); +std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); 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 df9640fa75626..f5b797978b744 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 @@ -60,7 +60,7 @@ class AvoidanceHelper geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } - size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( values.begin(), values.end(), [&](const auto value) { return velocity < value; }); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 72cfbe6f0a1ed..e23a8a96ee7da 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -19,11 +19,10 @@ #include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include +#include +#include #include -#include -#include #include namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 473b1d18c59ef..91c115706e030 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -20,9 +20,10 @@ #include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include +#include +#include #include #include @@ -30,11 +31,9 @@ #include #include -#include #include #include #include -#include #include namespace behavior_path_planner @@ -374,8 +373,8 @@ class AvoidanceModule : public SceneModuleInterface */ void removeRegisteredShiftLines() { - constexpr double THRESHOLD = 0.1; - if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + constexpr double threshold = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > threshold) { RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset registered shift lines."); return; } @@ -396,7 +395,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief remove behind shift lines. * @param path shifter. */ - void postProcess() + void postProcess() override { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); @@ -411,8 +410,6 @@ class AvoidanceModule : public SceneModuleInterface using RegisteredShiftLineArray = std::vector; - bool is_avoidance_maneuver_starts; - bool arrived_path_end_{false}; bool safe_{true}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index edfe9ab9663ce..efdda622a664c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -17,11 +17,8 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include - #include namespace behavior_path_planner::utils::avoidance @@ -29,8 +26,6 @@ namespace behavior_path_planner::utils::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::helper::avoidance::AvoidanceHelper; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; class ShiftLineGenerator { diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml index 642f83497acfb..f72adf0591493 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/behavior_path_avoidance_module/package.xml @@ -27,7 +27,6 @@ behavior_path_planner_common geometry_msgs lanelet2_extension - libboost-dev magic_enum motion_utils objects_of_interest_marker_interface diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 9d0492829c53b..ee36f8c33149d 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -14,6 +14,7 @@ #include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -22,9 +23,6 @@ #include -#include -#include - #include #include diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 952747cd64791..a05bcd90bdfb9 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -28,14 +29,11 @@ #include #include #include -#include -#include -#include -#include +#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include -#include #include #include @@ -1234,7 +1232,6 @@ void AvoidanceModule::initVariables() debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } @@ -1271,8 +1268,8 @@ void AvoidanceModule::updateRTCData() CandidateOutput output; - const auto sl_front = candidates.front(); - const auto sl_back = candidates.back(); + const auto & sl_front = candidates.front(); + const auto & sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; output.lateral_shift = helper_->getRelativeShiftToPath(shift_line); 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 4332008b95e70..01bfc375e9515 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -19,7 +19,7 @@ #include -#include +#include namespace behavior_path_planner::utils::avoidance { diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index f28ad324267cc..6abfc2e107348 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -21,29 +21,23 @@ #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include -#include #include -#include #include -#include +#include +#include -#include +#include #include #include #include -#include #include -#include #include #include #include #include -#include #include #include @@ -53,6 +47,7 @@ namespace behavior_path_planner::utils::avoidance using autoware_perception_msgs::msg::TrafficSignalElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; +using geometry_msgs::msg::TransformStamped; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -65,7 +60,6 @@ using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; using tier4_planning_msgs::msg::AvoidanceDebugFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsg; diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 58bf36d978cea..7b9a608aecec3 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -17,9 +17,6 @@ #include "planning_interface_test_manager/planning_interface_test_manager.hpp" #include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" -#include - -#include #include using behavior_path_planner::BehaviorPathPlannerNode; From cc0a1392b251ac194fc11ba34c48196e431e0b5f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Dec 2023 19:30:30 +0900 Subject: [PATCH 05/48] feat(avoidance): output objects of interest (#5929) Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index a05bcd90bdfb9..216021132f8a5 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1208,6 +1208,12 @@ void AvoidanceModule::updateData() // update rtc status. updateRTCData(); + // update interest objects data + for (const auto & [uuid, data] : debug_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + safe_ = avoid_data_.safe; } From fcbaba9e9c29f7ac14cd245f86c37e244d9d8648 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 22 Dec 2023 09:37:28 +0900 Subject: [PATCH 06/48] fix(avoidance): take objects on same root lanelet into consideration of safety check (#5828) fix(avoidance): use same root adjacent lane Signed-off-by: satoshi-ota --- .../data_structs.hpp | 3 +++ .../behavior_path_avoidance_module/utils.hpp | 3 ++- .../behavior_path_avoidance_module/src/debug.cpp | 2 ++ .../behavior_path_avoidance_module/src/scene.cpp | 2 +- .../behavior_path_avoidance_module/src/utils.cpp | 16 +++++++++++++--- 5 files changed, 21 insertions(+), 5 deletions(-) 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 be8bcb3fb970d..1d1494b7719c3 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 @@ -638,6 +638,9 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // safety check area + lanelet::ConstLanelets safety_check_lanes; + // collision check debug map utils::path_safety_checker::CollisionCheckDebugMap collision_check; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 9ce331e70d8b2..23a6ab5c52536 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -153,7 +153,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift); + const std::shared_ptr & parameters, const bool is_right_shift, + DebugData & debug); std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index ee36f8c33149d..ee97efe37d440 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -618,6 +618,8 @@ MarkerArray createDebugMarkerArray( createMarkerColor(0.16, 1.0, 0.69, 0.2))); add(laneletsAsTriangleMarkerArray( "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2))); + add(laneletsAsTriangleMarkerArray( + "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } return msg; diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 216021132f8a5..b03b9189dfca5 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -738,7 +738,7 @@ bool AvoidanceModule::isSafePath( const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, is_right_shift.value()); + avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6abfc2e107348..a4399c41bc0ef 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1654,12 +1654,12 @@ lanelet::ConstLanelets getAdjacentLane( lanelet::ConstLanelets lanes{}; for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); + const auto opt_left_lane = rh->getLeftLanelet(lane, true, false); if (!is_right_shift && opt_left_lane) { lanes.push_back(opt_left_lane.value()); } - const auto opt_right_lane = rh->getRightLanelet(lane); + const auto opt_right_lane = rh->getRightLanelet(lane, true, false); if (is_right_shift && opt_right_lane) { lanes.push_back(opt_right_lane.value()); } @@ -1675,7 +1675,8 @@ lanelet::ConstLanelets getAdjacentLane( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift) + const std::shared_ptr & parameters, const bool is_right_shift, + DebugData & debug) { const auto & p = parameters; const auto check_right_lanes = @@ -1733,6 +1734,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check left lanes @@ -1752,6 +1756,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check current lanes @@ -1771,6 +1778,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } return target_objects; From 18d2d5d69a6d34c5e4aba3097881470db2433288 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Dec 2023 08:01:45 +0900 Subject: [PATCH 07/48] fix(avoidance): add missing param declaration (#5948) Signed-off-by: satoshi-ota --- .../include/behavior_path_avoidance_module/parameter_helper.hpp | 2 ++ 1 file changed, 2 insertions(+) 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 eb5cb18ce697c..b9af50ce76eb5 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 @@ -207,6 +207,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "max_velocity"); p.ego_predicted_path_params.acceleration = getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); p.ego_predicted_path_params.time_resolution = From ebd76c68e3b9cfd45e13cb2a4423302b9a2d10a8 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Dec 2023 10:28:07 +0900 Subject: [PATCH 08/48] fix(avoidance): fix condition to trim front shift line (#5949) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/shift_line_generator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 01bfc375e9515..8ed56ce9aff4d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -890,7 +890,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { continue; } From 0fbeac0e3607d6bf8dd2df6cd83378fed808db49 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 26 Dec 2023 07:37:12 +0100 Subject: [PATCH 09/48] fix(planning_test_utils): rename header directory to match project name (#5863) * fix(planning_test_utils): rename header directory to match project name Signed-off-by: Esteve Fernandez * style(pre-commit): autofix Signed-off-by: Esteve Fernandez * fix: fix include Signed-off-by: Esteve Fernandez * fix: fix includes in downstream packages Signed-off-by: Esteve Fernandez * style(pre-commit): autofix Signed-off-by: Esteve Fernandez * fix: fix includes Signed-off-by: Esteve Fernandez * fix: fix includes Signed-off-by: Esteve Fernandez * fix: fix includes Signed-off-by: Esteve Fernandez --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../test/test_behavior_path_planner_node_interface.cpp | 7 ++++--- .../test/test_behavior_path_planner_node_interface.cpp | 7 ++++--- .../test/test_behavior_path_planner_node_interface.cpp | 4 ++-- .../test/test_behavior_path_planner_node_interface.cpp | 4 ++-- .../test/test_behavior_path_planner_node_interface.cpp | 7 ++++--- .../test/test_behavior_path_planner_node_interface.cpp | 7 ++++--- .../test/src/test_node_interface.cpp | 7 ++++--- .../test/test_freespace_planner_node_interface.cpp | 7 ++++--- .../test/test_motion_velocity_smoother_node_interface.cpp | 7 ++++--- .../test_obstacle_avoidance_planner_node_interface.cpp | 7 ++++--- .../test/test_obstacle_cruise_planner_node_interface.cpp | 7 ++++--- .../test/test_obstacle_stop_planner_node_interface.cpp | 7 ++++--- .../test/test_obstacle_velocity_limiter_node_interface.cpp | 7 ++++--- .../test/test_path_smoother_node_interface.cpp | 7 ++++--- .../planning_interface_test_manager.hpp | 6 +++--- .../planning_interface_test_manager_utils.hpp | 6 +++--- .../src/planning_interface_test_manager.cpp | 4 ++-- .../test/src/test_planning_validator_node_interface.cpp | 7 ++++--- .../test/test_scenario_selector_node_interface.cpp | 7 ++++--- 19 files changed, 68 insertions(+), 54 deletions(-) rename planning/planning_test_utils/include/{planning_interface_test_manager => planning_test_utils}/planning_interface_test_manager.hpp (98%) rename planning/planning_test_utils/include/{planning_interface_test_manager => planning_test_utils}/planning_interface_test_manager_utils.hpp (98%) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 3041ae7e1112e..581aafa6c860c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 7b9a608aecec3..8cf85554e2b2c 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index ff5be388704da..434988cc3ab08 100644 --- a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -14,8 +14,8 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include diff --git a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 7a606b5d126ce..82f721411d5a4 100644 --- a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -14,8 +14,8 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index f2c22f774fc77..28ee6d31e80dc 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index 94be083bdebcd..eb7d1afe27549 100644 --- a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 491df4c7a8766..239abbde27609 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp index 8c6336b2f45d6..013eb3c33cbad 100644 --- a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "freespace_planner/freespace_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp b/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp index 4c8eaac67cbb5..430f8b78ec88c 100644 --- a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp +++ b/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp index 75b61f80622d1..9c567487e9cac 100644 --- a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp +++ b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "obstacle_avoidance_planner/node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index 5be04ab801e14..d412286d77d53 100644 --- a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "obstacle_cruise_planner/node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp index 95afdd7f2ea3b..7f986bf848777 100644 --- a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp +++ b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "obstacle_stop_planner/node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp index c2ffaffc75287..a89042ef210d5 100644 --- a/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp +++ b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/path_smoother/test/test_path_smoother_node_interface.cpp b/planning/path_smoother/test/test_path_smoother_node_interface.cpp index b18c49b0e88b0..3598e07f84fd6 100644 --- a/planning/path_smoother/test/test_path_smoother_node_interface.cpp +++ b/planning/path_smoother/test/test_path_smoother_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "path_smoother/elastic_band_smoother.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager.hpp similarity index 98% rename from planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp rename to planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager.hpp index 556ef9a4a4afa..a63b0d02152f0 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ -#define PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +#ifndef PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +#define PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_HPP_ // since ASSERT_NO_THROW in gtest masks the exception message, redefine it. #define ASSERT_NO_THROW_WITH_ERROR_MSG(statement) \ @@ -266,4 +266,4 @@ class PlanningInterfaceTestManager } // namespace planning_test_utils -#endif // PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +#endif // PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_HPP_ diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp similarity index 98% rename from planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp rename to planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp index 0af375c9d9c8a..4d8d7be96b44b 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ -#define PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ +#ifndef PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ +#define PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ #include "ament_index_cpp/get_package_share_directory.hpp" #include "rclcpp/rclcpp.hpp" @@ -569,4 +569,4 @@ PathWithLaneId loadPathWithLaneIdInYaml() } // namespace test_utils -#endif // PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ +#endif // PLANNING_TEST_UTILS__PLANNING_INTERFACE_TEST_MANAGER_UTILS_HPP_ diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index 6489f32dc4cdf..a9d096bf00fd4 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include namespace planning_test_utils { diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 95619e749a9f9..606dc182504a2 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" #include "planning_validator/planning_validator.hpp" +#include +#include +#include + #include #include diff --git a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp index d3d6939f49b83..7413be07ef904 100644 --- a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp +++ b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" #include "scenario_selector/scenario_selector_node.hpp" +#include +#include +#include + #include #include From fd9524ae99cace2be55bf354c72d3cd321e2852d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Dec 2023 17:55:55 +0900 Subject: [PATCH 10/48] fix(avoidance): don't ignore objects on straight lane in intersection (#5939) * fix(avoidance): don't ignore objects on straight lane in intersection Signed-off-by: satoshi-ota * fix(avoidance): fix filtering flow Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 36 +++++++++---------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index a4399c41bc0ef..ed7063c9fecba 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -635,18 +635,7 @@ bool isSatisfiedWithVehicleCondition( return true; } - // always ignore all merging objects. - if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; - return false; - } - - // Object is on center line -> ignore. - if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; - return false; - } - + // check vehicle shift ratio lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); const auto on_ego_driving_lane = within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); @@ -659,17 +648,26 @@ bool isSatisfiedWithVehicleCondition( } } - if (!object.is_within_intersection) { - return true; + // Object is on center line -> ignore. + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; } - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { - return true; + if (object.is_within_intersection) { + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "straight") { + return true; + } + + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + return false; + } } - if (object.behavior == ObjectData::Behavior::NONE) { - object.reason = "ParallelToEgoLane"; + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; return false; } From a384d2aad0e7c9cb38a5662df4f0708372561e80 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Dec 2023 17:56:05 +0900 Subject: [PATCH 11/48] feat(avoidance): suppress unnatural turn signal (#5905) * refactor(avoidance): clean up Signed-off-by: satoshi-ota * fix(avoidance): suppress unnecessary turn signal Signed-off-by: satoshi-ota * refactor(avoidance): move utils Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/scene.hpp | 7 - .../behavior_path_avoidance_module/utils.hpp | 4 + .../src/scene.cpp | 123 +----------- .../src/utils.cpp | 189 ++++++++++++++++++ 4 files changed, 200 insertions(+), 123 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 91c115706e030..889d48c481b6f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -325,13 +325,6 @@ class AvoidanceModule : public SceneModuleInterface // generate output data - /** - * @brief calculate turn signal infomation. - * @param avoidance path. - * @return turn signal command. - */ - TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - /** * @brief fill debug markers. */ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 23a6ab5c52536..4508cbc3c18f6 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -174,6 +174,10 @@ double calcDistanceToAvoidStartLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + +TurnSignalInfo calcTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, + const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b03b9189dfca5..31849c716971c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -17,7 +17,6 @@ #include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -874,7 +873,7 @@ BehaviorModuleOutput AvoidanceModule::plan() if (success_spline_path_generation && success_linear_path_generation) { helper_->setPreviousLinearShiftPath(linear_shift_path); helper_->setPreviousSplineShiftPath(spline_shift_path); - helper_->setPreviousReferencePath(data.reference_path); + helper_->setPreviousReferencePath(path_shifter_.getReferencePath()); } else { spline_shift_path = helper_->getPreviousSplineShiftPath(); } @@ -887,9 +886,13 @@ BehaviorModuleOutput AvoidanceModule::plan() BehaviorModuleOutput output; // turn signal info - { + if (path_shifter_.getShiftLines().empty()) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(spline_shift_path); + const auto new_signal = utils::avoidance::calcTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, + planner_data_); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -1285,118 +1288,6 @@ void AvoidanceModule::updateRTCData() updateCandidateRTCStatus(output); } -TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const -{ - const auto shift_lines = path_shifter_.getShiftLines(); - if (shift_lines.empty()) { - return {}; - } - - const auto front_shift_line = shift_lines.front(); - const size_t start_idx = front_shift_line.start_idx; - const size_t end_idx = front_shift_line.end_idx; - - const auto current_shift_length = helper_->getEgoShift(); - const double start_shift_length = path.shift_length.at(start_idx); - const double end_shift_length = path.shift_length.at(end_idx); - const double segment_shift_length = end_shift_length - start_shift_length; - - const double turn_signal_shift_length_threshold = - planner_data_->parameters.turn_signal_shift_length_threshold; - const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - const double turn_signal_minimum_search_distance = - planner_data_->parameters.turn_signal_minimum_search_distance; - - // If shift length is shorter than the threshold, it does not need to turn on blinkers - if (std::fabs(segment_shift_length) < turn_signal_shift_length_threshold) { - return {}; - } - - // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < 0.1) { - return {}; - } - - // compute blinker start idx and end idx - size_t blinker_start_idx = [&]() { - for (size_t idx = start_idx; idx <= end_idx; ++idx) { - const double current_shift_length = path.shift_length.at(idx); - if (current_shift_length > 0.1) { - return idx; - } - } - return start_idx; - }(); - size_t blinker_end_idx = end_idx; - - // prevent invalid access for out-of-range - blinker_start_idx = - std::min(std::max(std::size_t(0), blinker_start_idx), path.path.points.size() - 1); - blinker_end_idx = - std::min(std::max(blinker_start_idx, blinker_end_idx), path.path.points.size() - 1); - - const auto blinker_start_pose = path.path.points.at(blinker_start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(blinker_end_idx).point.pose; - - const double ego_vehicle_offset = - planner_data_->parameters.vehicle_info.max_longitudinal_offset_m; - const auto signal_prepare_distance = - std::max(getEgoSpeed() * turn_signal_search_time, turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, getEgoPosition(), blinker_start_pose.position) - - ego_vehicle_offset; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; - } - - bool turn_signal_on_swerving = planner_data_->parameters.turn_signal_on_swerving; - - TurnSignalInfo turn_signal_info{}; - if (turn_signal_on_swerving) { - if (segment_shift_length > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } else { - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); - const auto local_vehicle_footprint = - createVehicleFootprint(planner_data_->parameters.vehicle_info); - boost::geometry::model::ring shifted_vehicle_footprint; - for (const auto & cl : current_lanes) { - // get left and right bounds of current lane - const auto lane_left_bound = cl.leftBound2d().basicLineString(); - const auto lane_right_bound = cl.rightBound2d().basicLineString(); - for (size_t i = start_idx; i < end_idx; ++i) { - // transform vehicle footprint onto path points - shifted_vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); - if ( - boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || - boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { - if (segment_shift_length > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } - } - } - } - if (ego_front_to_shift_start > 0.0) { - turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; - } else { - turn_signal_info.desired_start_point = blinker_start_pose; - } - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - - return turn_signal_info; -} - void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index ed7063c9fecba..493ca35f09a3b 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -16,6 +16,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" @@ -225,6 +226,98 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } +bool existShiftSideLane( + const double start_shift_length, const double end_shift_length, const bool no_left_lanes, + const bool no_right_lanes) +{ + constexpr double THRESHOLD = 0.1; + const auto relative_shift_length = end_shift_length - start_shift_length; + + const auto avoid_shift = + std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD; + if (avoid_shift) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + } + + const auto return_shift = + std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD; + if (return_shift) { + // Right return. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD; + if (left_middle_shift) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD; + if (right_middle_shift) { + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + } + + return true; +} + +bool straddleRoadBound( + const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { + const auto transform = pose2transform(path.path.points.at(i).point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; +} + } // namespace namespace filtering_utils @@ -2055,4 +2148,100 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } + +TurnSignalInfo calcTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, + const AvoidancePlanningData & data, const std::shared_ptr & planner_data) +{ + constexpr double THRESHOLD = 0.1; + const auto & p = planner_data->parameters; + const auto & rh = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x; + + if (shift_line.start_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.start_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.end_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.end_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + const auto start_shift_length = path.shift_length.at(shift_line.start_idx); + const auto end_shift_length = path.shift_length.at(shift_line.end_idx); + const auto relative_shift_length = end_shift_length - start_shift_length; + + // If shift length is shorter than the threshold, it does not need to turn on blinkers + if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { + return {}; + } + + // If the vehicle does not shift anymore, we turn off the blinker + if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { + return {}; + } + + const auto get_command = [](const auto & shift_length) { + return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT + : TurnIndicatorsCommand::ENABLE_RIGHT; + }; + + const auto signal_prepare_distance = + std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); + const auto ego_front_to_shift_start = + calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - + p.vehicle_info.max_longitudinal_offset_m; + + if (signal_prepare_distance < ego_front_to_shift_start) { + return {}; + } + + const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; + const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; + const auto get_start_pose = [&](const auto & ego_to_shift_start) { + return ego_to_shift_start ? ego_pose : blinker_start_pose; + }; + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); + turn_signal_info.desired_end_point = blinker_end_pose; + turn_signal_info.required_start_point = blinker_start_pose; + turn_signal_info.required_end_point = blinker_end_pose; + turn_signal_info.turn_signal.command = get_command(relative_shift_length); + + if (!p.turn_signal_on_swerving) { + return turn_signal_info; + } + + lanelet::ConstLanelet lanelet; + if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { + return {}; + } + + const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); + const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + + if (!existShiftSideLane( + start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + return {}; + } + + if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { + return {}; + } + + return turn_signal_info; +} } // namespace behavior_path_planner::utils::avoidance From 3c1ea92717f875c7548fa34f534828851964fb8c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 11 Jan 2024 00:32:30 +0900 Subject: [PATCH 12/48] feat(avoidance): check traffic light info in order to limit drivable area (#6016) * feat(avoidance): don't use opposite lane before intersection Signed-off-by: satoshi-ota * feat(avoidance): check traffic light info in order to limit drivable area Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 2 +- .../behavior_path_avoidance_module/utils.hpp | 2 +- .../src/scene.cpp | 6 ++++-- .../src/utils.cpp | 12 ++++++++++- .../utils/traffic_light_utils.hpp | 15 ++++++++++++++ .../src/utils/traffic_light_utils.cpp | 20 +++++++++++++++++++ 6 files changed, 52 insertions(+), 5 deletions(-) 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 e245f1c44fe0e..eb0919611be8d 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 @@ -163,7 +163,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( - lanelet, planner_data_, avoidance_parameters_)); + lanelet, planner_data_, avoidance_parameters_, false)); }); // calc drivable bound diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 4508cbc3c18f6..dc602edcc8b86 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -163,7 +163,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters, const bool in_avoidance_maneuver); double calcDistanceToReturnDeadLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 31849c716971c..9d7f5cc459e9e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -223,10 +223,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes + const auto has_shift_point = !path_shifter_.getShiftLines().empty(); + const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, parameters_, in_avoidance_maneuver)); }); // calc drivable bound diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 493ca35f09a3b..83e96d83b995f 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1953,7 +1953,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters, const bool in_avoidance_maneuver) { const auto & route_handler = planner_data->route_handler; @@ -1967,6 +1967,11 @@ DrivableLanes generateExpandDrivableLanes( // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_left_lanelets.empty()) { @@ -1977,6 +1982,11 @@ DrivableLanes generateExpandDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_right_lanelets.empty()) { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index bf48252a01dcd..e834443a40a51 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -129,6 +129,21 @@ bool isStoppedAtRedTrafficLightWithinDistance( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const double distance_threshold = std::numeric_limits::infinity()); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::traffic_light #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 850919f539e59..88adfc8820787 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -177,4 +177,24 @@ bool isStoppedAtRedTrafficLightWithinDistance( return (distance_to_red_traffic_light < distance_threshold); } +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { + return true; + } + } + } + + return false; +} + } // namespace behavior_path_planner::utils::traffic_light From d22568fa3f0924c2d565c2bcdd1fbafe8a0e417e Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 8 Apr 2024 09:30:12 +0900 Subject: [PATCH 13/48] fixup! feat(avoidance): check traffic light info in order to limit drivable area (#6016) --- .../src/utils/traffic_light_utils.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 88adfc8820787..9deaf6c927d69 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -187,8 +187,7 @@ bool isTrafficSignalStop( continue; } - if (traffic_light_utils::isTrafficSignalStop( - lanelet, traffic_signal_stamped.value().signal)) { + if (isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { return true; } } From 6e9dccbdcec135fcecf36778c847eb3a7ff5fa4f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 11 Jan 2024 01:39:06 +0900 Subject: [PATCH 14/48] fix(avoidance): return shift path was not generated expectedly (#6017) fix(avoidance): output return shift path properly Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 3 ++- .../src/shift_line_generator.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 9d7f5cc459e9e..61941eeee6f00 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1390,7 +1390,8 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); + const auto min_return_distance = + helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point 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 8ed56ce9aff4d..dabb0d7257555 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -1101,12 +1101,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); + double prepare_distance_scaled = + std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1219,7 +1221,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { break; } From ac5ca1acd8808dbd5cbc6fb2d3e37c24cab705e2 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 15 Jan 2024 19:59:36 +0900 Subject: [PATCH 15/48] perf(bpp): reduce computational cost (#6054) * pref(avoidance): don't use boost union_ Signed-off-by: satoshi-ota * perf(avoidance): reduce frequency to call calcSignedArcLength Signed-off-by: satoshi-ota * perf(turn_signal): reduce frequency to call calcSignedArcLength Signed-off-by: satoshi-ota * perf(static_drivable_area_expansion): don't use calcDistance2d Signed-off-by: satoshi-ota * fix(static_drivable_area_expansion): rename and fix return value consistency Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../data_structs.hpp | 2 +- .../behavior_path_avoidance_module/scene.hpp | 13 ++-- .../src/debug.cpp | 7 ++- .../src/scene.cpp | 62 +++++++++++-------- .../src/utils.cpp | 45 +++++--------- .../src/turn_signal_decider.cpp | 18 +++--- .../static_drivable_area.cpp | 17 +++-- 7 files changed, 84 insertions(+), 80 deletions(-) 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 1d1494b7719c3..86e1608501831 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 @@ -588,7 +588,7 @@ struct ShiftLineData */ struct DebugData { - geometry_msgs::msg::Polygon detection_area; + std::vector detection_areas; lanelet::ConstLineStrings3d bounds; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 889d48c481b6f..0bb480bfa26b1 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRegisteredRTCStatus(const PathWithLaneId & path) { - const Point ego_position = planner_data_->self_odometry->pose.pose.position; + const auto ego_idx = planner_data_->findEgoIndex(path.points); for (const auto & left_shift : left_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface for (const auto & right_shift : right_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface UUID uuid; Pose start_pose; Pose finish_pose; + double relative_longitudinal{0.0}; }; using RegisteredShiftLineArray = std::vector; diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index ee97efe37d440..f088b9228d964 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray( addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } + // detection area + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } + // misc { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 61941eeee6f00..c94243451ed2b 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -703,18 +703,6 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const bool limit_to_max_velocity = false; - const auto ego_predicted_path_params = - std::make_shared( - parameters_->ego_predicted_path_params); - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); - const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - true, limit_to_max_velocity); - const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - false, limit_to_max_velocity); - const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { @@ -741,6 +729,22 @@ bool AvoidanceModule::isSafePath( const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); + if (safety_check_target_objects.empty()) { + return true; + } + + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); + for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); @@ -793,15 +797,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); + auto lines = path_shifter_.getShiftLines(); + if (lines.empty()) { + return max_dist; } - for (const auto & sp : generator_.getRawRegisteredShiftLine()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); - } - return max_dist; + std::sort(lines.begin(), lines.end(), [](const auto & a, const auto & b) { + return a.start_idx < b.start_idx; + }); + return std::max( + max_dist, + calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition())); }(); const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. @@ -1029,11 +1034,14 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) const auto sl = helper_->getMainShiftLine(shift_lines); const auto sl_front = shift_lines.front(); const auto sl_back = shift_lines.back(); + const auto relative_longitudinal = sl_back.end_longitudinal - sl_front.start_longitudinal; if (helper_->getRelativeShiftToPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); + left_shift_array_.push_back( + {uuid_map_.at("left"), sl_front.start, sl_back.end, relative_longitudinal}); } else if (helper_->getRelativeShiftToPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); + right_shift_array_.push_back( + {uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal}); } uuid_map_.at("left") = generateUUID(); @@ -1150,15 +1158,19 @@ bool AvoidanceModule::isValidShiftLine( const size_t start_idx = shift_lines.front().start_idx; const size_t end_idx = shift_lines.back().end_idx; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound); + const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound); for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + const auto p = getPoint(path.points.at(i)); lanelet::BasicPoint2d basic_point{p.x, p.y}; const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; const auto THRESHOLD = minimum_distance + std::abs(shift_length); - if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 1000, "following latest new shift line may cause deviation from drivable area."); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 83e96d83b995f..1fbc18f84d86d 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1893,11 +1893,12 @@ std::pair separateObjectsByPath( max_offset = std::max(max_offset, offset); } + const double MARGIN = is_running ? 1.0 : 0.0; // [m] const auto detection_area = - createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); + createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset + MARGIN); const auto ego_idx = planner_data->findEgoIndex(path.points); - Polygon2d attention_area; + std::vector detection_areas; for (size_t i = 0; i < path.points.size() - 1; ++i) { const auto & p_ego_front = path.points.at(i).point.pose; const auto & p_ego_back = path.points.at(i + 1).point.pose; @@ -1907,41 +1908,27 @@ std::pair separateObjectsByPath( break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area); - - std::vector unions; - boost::geometry::union_(attention_area, ego_one_step_polygon, unions); - if (!unions.empty()) { - attention_area = unions.front(); - boost::geometry::correct(attention_area); - } + detection_areas.push_back(createOneStepPolygon(p_ego_front, p_ego_back, detection_area)); } - // expand detection area width only when the module is running. - if (is_running) { - constexpr int PER_CIRCLE = 36; - constexpr double MARGIN = 1.0; // [m] - boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); - boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - if (!result.empty()) { - attention_area = result.front(); + std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) { + debug.detection_areas.push_back(toMsg(detection_area, data.reference_pose.position.z)); + }); + + const auto within_detection_area = [&](const auto & obj_polygon) { + for (const auto & detection_area : detection_areas) { + if (!boost::geometry::disjoint(obj_polygon, detection_area)) { + return true; + } } - } - debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); + return false; + }; const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - if (boost::geometry::disjoint(obj_polygon, attention_area)) { + if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { target_objects.objects.push_back(object); diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 933330dafae7e..0c105e804693a 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -415,15 +415,7 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const double dist_to_original_desired_start = get_distance(original_desired_start_point) - base_link2front_; - const double dist_to_original_desired_end = get_distance(original_desired_end_point); - const double dist_to_original_required_start = - get_distance(original_required_start_point) - base_link2front_; - const double dist_to_original_required_end = get_distance(original_required_end_point); const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; - const double dist_to_new_desired_end = get_distance(new_desired_end_point); - const double dist_to_new_required_start = - get_distance(new_required_start_point) - base_link2front_; - const double dist_to_new_required_end = get_distance(new_required_end_point); // If we still do not reach the desired front point we ignore it if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { @@ -435,6 +427,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + // If we already passed the desired end point, return the other signal if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { TurnSignalInfo empty_signal_info; @@ -445,6 +440,13 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + if (dist_to_original_desired_start <= dist_to_new_desired_start) { const auto enable_prior = use_prior_turn_signal( dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, 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 af4bebe183d1d..700263851e21a 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 @@ -128,26 +128,25 @@ std::optional> intersectBound( return std::nullopt; } -double calcDistanceFromPointToSegment( +double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_start_point, const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcSquaredDistance2d; + const auto & a = segment_start_point; const auto & b = segment_end_point; const auto & p = target_point; const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); - const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + const double squared_segment_length = calcSquaredDistance2d(a, b); if (0 <= dot_val && dot_val <= squared_segment_length) { - const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); - const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); - return numerator / denominator; + return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length; } // target_point is outside the segment. - return std::min( - tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); + return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p)); } PolygonPoint transformBoundFrenetCoordinate( @@ -158,8 +157,8 @@ PolygonPoint transformBoundFrenetCoordinate( // find wrong nearest index. std::vector dist_to_bound_segment_vec; for (size_t i = 0; i < bound_points.size() - 1; ++i) { - const double dist_to_bound_segment = - calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment( + bound_points.at(i), bound_points.at(i + 1), target_point); dist_to_bound_segment_vec.push_back(dist_to_bound_segment); } From de9c302835b2ec3633617124bfd0a192562dc2f3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 16 Jan 2024 14:26:04 +0900 Subject: [PATCH 16/48] feat(avoidance): improve object detection area in order not to prevent endless loop (#6084) * perf(avoidance): reduce heavy process Signed-off-by: satoshi-ota * fix(avoidance): filter objects by precise lon distance Signed-off-by: satoshi-ota * refactor(avoidance): remove unused function Signed-off-by: satoshi-ota * feat(avoidance): improve detection area Signed-off-by: satoshi-ota * fix(avoidance): return shift line Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +- .../behavior_path_avoidance_module/utils.hpp | 19 +- .../src/scene.cpp | 32 +-- .../src/shift_line_generator.cpp | 8 +- .../src/utils.cpp | 203 +++++++++++------- 5 files changed, 143 insertions(+), 125 deletions(-) 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 eb0919611be8d..9a4c4c80a256e 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 @@ -184,7 +184,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( } void AvoidanceByLaneChange::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -224,7 +224,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [&](const auto & object) { return createObjectData(data, object); }); } - utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p); + utils::avoidance::filterTargetObjects( + target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance, + planner_data_, p); } ObjectData AvoidanceByLaneChange::createObjectData( diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index dc602edcc8b86..cbf68ada44abb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -77,11 +77,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); - -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); @@ -128,12 +123,7 @@ void compensateDetectionLost( ObjectDataArray & other_objects); void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); - -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); @@ -157,9 +147,10 @@ std::vector getSafetyCheckTargetObjects( DebugData & debug); std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug); + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index c94243451ed2b..bc1d8c6d75424 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -287,21 +287,18 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; - - // Add margin in order to prevent avoidance request chattering only when the module is running. - const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || - getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + using utils::avoidance::separateObjectsByPath; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto sparse_resample_path = utils::resamplePathWithSpline( - helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::avoidance::separateObjectsByPath( - sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(), - is_running, debug); + constexpr double MARGIN = 10.0; + const auto forward_detection_range = helper_->getForwardDetectionRange(); + 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); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -316,11 +313,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } // Filter out the objects to determine the ones to be avoided. - filterTargetObjects(objects, data, debug, planner_data_, parameters_); + filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); o.to_stop_line = calcDistanceToStopLine(o); fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); }); @@ -380,21 +379,10 @@ ObjectData AvoidanceModule::createObjectData( utils::avoidance::fillInitialPose(object_data, detected_objects_); // Calc lateral deviation from path to target object. - object_data.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, data.reference_path, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity( - object_data, registered_objects_, vehicle_width, parameters_); - return object_data; } 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 dabb0d7257555..c20a8a73c955d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -1101,8 +1101,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = - std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); + double prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / @@ -1122,7 +1122,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; - al.end_longitudinal = arclength_from_ego.at(al.end_idx); + al.end_longitudinal = prepare_distance_scaled; al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; ret.push_back(al); @@ -1136,7 +1136,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; - al.start_longitudinal = arclength_from_ego.at(al.start_idx); + al.start_longitudinal = prepare_distance_scaled; al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 1fbc18f84d86d..31380c6a1d769 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -176,7 +176,8 @@ geometry_msgs::msg::Polygon createVehiclePolygon( } Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, + const geometry_msgs::msg::Pose & p3, const geometry_msgs::msg::Pose & p4, const geometry_msgs::msg::Polygon & base_polygon) { Polygon2d one_step_polygon{}; @@ -184,7 +185,7 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); + geometry_tf.transform = pose2transform(p1); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -195,7 +196,29 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); + geometry_tf.transform = pose2transform(p2); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p3); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p4); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -630,7 +653,7 @@ bool isForceAvoidanceTarget( } bool isSatisfiedWithCommonCondition( - ObjectData & object, const AvoidancePlanningData & data, + ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -657,7 +680,7 @@ bool isSatisfiedWithCommonCondition( return false; } - if (object.longitudinal > parameters->object_check_max_forward_distance) { + if (object.longitudinal > forward_detection_range) { object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; return false; } @@ -742,6 +765,9 @@ bool isSatisfiedWithVehicleCondition( } // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; return false; @@ -818,6 +844,64 @@ std::optional getAvoidMargin( // Step3. nominal case. avoid margin is limited by soft constraint. return std::min(soft_lateral_distance_limit, max_avoid_margin); } + +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data) +{ + using lanelet::utils::to2D; + using tier4_autoware_utils::Point2d; + + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + findNearestIndex(data.reference_path.points, object_pose.position); + const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; + + const auto rh = planner_data->route_handler; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { + return 0.0; + } + + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto & p1_object = object.overhang_pose.position; + const auto p_tmp = + geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); + const auto p2_object = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + std::vector intersects; + for (size_t i = 1; i < bound.size(); i++) { + const auto p1_bound = + geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); + const auto p2_bound = + geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); + + const auto opt_intersect = + tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); + + if (!opt_intersect) { + continue; + } + + intersects.push_back(opt_intersect.value()); + } + + if (intersects.empty()) { + return 0.0; + } + + std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { + return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); + }); + + object.nearest_bound_point = intersects.front(); + + return calcDistance2d(p1_object, object.nearest_bound_point.value()); +} } // namespace filtering_utils bool isOnRight(const ObjectData & obj) @@ -1114,11 +1198,6 @@ std::vector generateObstaclePolygonsForDrivableArea( return obstacles_for_drivable_area; } -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) -{ - return v * std::cos(calcYawDeviation(p_ref, p_target)); -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1518,67 +1597,8 @@ void compensateDetectionLost( } } -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - [[maybe_unused]] const std::shared_ptr & parameters) -{ - using lanelet::utils::to2D; - using tier4_autoware_utils::Point2d; - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = - findNearestIndex(data.reference_path.points, object_pose.position); - const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; - - const auto rh = planner_data->route_handler; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { - return 0.0; - } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto & p1_object = object.overhang_pose.position; - const auto p_tmp = - geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); - const auto p2_object = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; - - // TODO(Satoshi OTA): check if the basic point is on right or left of bound. - const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; - - std::vector intersects; - for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); - - const auto opt_intersect = - tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); - - if (!opt_intersect) { - continue; - } - - intersects.push_back(opt_intersect.value()); - } - - if (intersects.empty()) { - return 0.0; - } - - std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { - return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); - }); - - object.nearest_bound_point = intersects.front(); - - return calcDistance2d(p1_object, object.nearest_bound_point.value()); -} - void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -1593,12 +1613,16 @@ void filterTargetObjects( }; for (auto & o : objects) { - if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + if (!filtering_utils::isSatisfiedWithCommonCondition( + o, data, forward_detection_range, planner_data, parameters)) { data.other_objects.push_back(o); continue; } - o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); + // Find the footprint point closest to the path, set to object_data.overhang_distance. + o.overhang_dist = + calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position); + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { @@ -1878,9 +1902,10 @@ std::vector getSafetyCheckTargetObjects( } std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug) + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1893,22 +1918,34 @@ std::pair separateObjectsByPath( max_offset = std::max(max_offset, offset); } - const double MARGIN = is_running ? 1.0 : 0.0; // [m] const auto detection_area = - createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset + MARGIN); - const auto ego_idx = planner_data->findEgoIndex(path.points); + createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); + const auto ego_idx = planner_data->findEgoIndex(reference_path.points); + const auto arc_length_array = + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 1e-3); + double next_longitudinal_distance = 0.0; std::vector detection_areas; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p_ego_front = path.points.at(i).point.pose; - const auto & p_ego_back = path.points.at(i + 1).point.pose; + for (size_t i = 0; i < reference_path.points.size() - 1; ++i) { + const auto & p_reference_ego_front = reference_path.points.at(i).point.pose; + const auto & p_reference_ego_back = reference_path.points.at(i + 1).point.pose; + const auto & p_spline_ego_front = spline_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i + 1).point.pose; - const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); + const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } - detection_areas.push_back(createOneStepPolygon(p_ego_front, p_ego_back, detection_area)); + if (arc_length_array.at(i) < next_longitudinal_distance) { + continue; + } + + detection_areas.push_back(createOneStepPolygon( + p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, + detection_area)); + + next_longitudinal_distance += parameters->resample_interval_for_output; } std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) { From 52bcb790e1242337d395a05ace0096ad51f95cf2 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 17 Jan 2024 18:14:36 +0900 Subject: [PATCH 17/48] fix(avoidance): fix detection area issue in avoidance module (#6097) * fix(avoidance): fix invalid vector access Signed-off-by: satoshi-ota * fix(avoidance): fix detection area Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 31380c6a1d769..b7e5cfe8cee36 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1910,6 +1910,10 @@ std::pair separateObjectsByPath( PredictedObjects target_objects; PredictedObjects other_objects; + if (reference_path.points.empty() || spline_path.points.empty()) { + return std::make_pair(target_objects, other_objects); + } + double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; @@ -1922,16 +1926,15 @@ std::pair separateObjectsByPath( createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); const auto ego_idx = planner_data->findEgoIndex(reference_path.points); const auto arc_length_array = - utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 1e-3); + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 0.0); - double next_longitudinal_distance = 0.0; - std::vector detection_areas; - for (size_t i = 0; i < reference_path.points.size() - 1; ++i) { - const auto & p_reference_ego_front = reference_path.points.at(i).point.pose; - const auto & p_reference_ego_back = reference_path.points.at(i + 1).point.pose; - const auto & p_spline_ego_front = spline_path.points.at(i).point.pose; - const auto & p_spline_ego_back = spline_path.points.at(i + 1).point.pose; + const auto points_size = std::min(reference_path.points.size(), spline_path.points.size()); + std::vector detection_areas; + Pose p_reference_ego_front = reference_path.points.front().point.pose; + Pose p_spline_ego_front = spline_path.points.front().point.pose; + double next_longitudinal_distance = parameters->resample_interval_for_output; + for (size_t i = 0; i < points_size; ++i) { const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; @@ -1941,10 +1944,16 @@ std::pair separateObjectsByPath( continue; } + const auto & p_reference_ego_back = reference_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i).point.pose; + detection_areas.push_back(createOneStepPolygon( p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, detection_area)); + p_reference_ego_front = p_reference_ego_back; + p_spline_ego_front = p_spline_ego_back; + next_longitudinal_distance += parameters->resample_interval_for_output; } From 5a23b8588041a939167bed8908d92e7c41eded04 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sat, 27 Jan 2024 14:12:43 +0900 Subject: [PATCH 18/48] fix(avoidance): turn signal doesn't turn on (#6188) Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/utils.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index b7e5cfe8cee36..f1eb370efd269 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -2273,11 +2273,11 @@ TurnSignalInfo calcTurnSignalInfo( return {}; } - const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); - const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + const auto left_lane = rh->getLeftLanelet(lanelet, true, true); + const auto right_lane = rh->getRightLanelet(lanelet, true, true); if (!existShiftSideLane( - start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + start_shift_length, end_shift_length, !left_lane.has_value(), !right_lane.has_value())) { return {}; } From 2db0351f65778225594f8f9ca45f19c694fb684a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:11:30 +0900 Subject: [PATCH 19/48] fix(avoidance): fix bug in turn signal decision (#6193) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/utils.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index f1eb370efd269..f7ffd37e647f0 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -2273,11 +2273,15 @@ TurnSignalInfo calcTurnSignalInfo( return {}; } - const auto left_lane = rh->getLeftLanelet(lanelet, true, true); - const auto right_lane = rh->getRightLanelet(lanelet, true, true); - - if (!existShiftSideLane( - start_shift_length, end_shift_length, !left_lane.has_value(), !right_lane.has_value())) { + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); + + if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { return {}; } From 0dc298f16a1e239c98c285586a80f4cb00c39a68 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 30 Jan 2024 11:04:06 +0900 Subject: [PATCH 20/48] fix(avoidance): update filtering logic for non-vehicle (#6206) Signed-off-by: satoshi-ota --- .../src/debug.cpp | 1 + .../src/utils.cpp | 29 ++++++++++++++++--- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index f088b9228d964..7c55c62bd8cc1 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -557,6 +557,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("UnstableObject")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index f7ffd37e647f0..e485e70c45625 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -369,13 +369,15 @@ bool isSafetyCheckTargetObjectType( return parameters->object_parameters.at(object_type).is_safety_check_target; } -bool isVehicleTypeObject(const ObjectData & object) +bool isUnknownTypeObject(const ObjectData & object) { const auto object_type = utils::getHighestProbLabel(object.object.classification); + return object_type == ObjectClassification::UNKNOWN; +} - if (object_type == ObjectClassification::UNKNOWN) { - return false; - } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); if (object_type == ObjectClassification::PEDESTRIAN) { return false; @@ -722,6 +724,15 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + return true; } @@ -1625,6 +1636,16 @@ void filterTargetObjects( 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] + if (filtering_utils::isUnknownTypeObject(o)) { + if (o.stop_time < STOP_TIME_THRESHOLD) { + o.reason = "UnstableObject"; + data.other_objects.push_back(o); + continue; + } + } + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; From 5ecfb3f09a87577b403f2f8f0ad14fddde458a59 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 31 Jan 2024 17:08:54 +0900 Subject: [PATCH 21/48] refactor(avoidance): add function to check if the object is moving (#6243) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/scene.cpp | 3 +-- .../behavior_path_avoidance_module/src/utils.cpp | 12 +++++++++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index bc1d8c6d75424..4debd2aaad526 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -301,8 +301,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; + ObjectData other_object = createObjectData(data, object); other_object.reason = "OutOfTargetArea"; data.other_objects.push_back(other_object); } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index e485e70c45625..81a3e1004f418 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -390,6 +390,14 @@ bool isVehicleTypeObject(const ObjectData & object) return true; } +bool isMovingObject( + const ObjectData & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + return object.move_time > object_parameter.moving_time_threshold; +} + bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs) @@ -666,9 +674,7 @@ bool isSatisfiedWithCommonCondition( } // Step2. filtered stopped objects. - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - if (object.move_time > object_parameter.moving_time_threshold) { + if (filtering_utils::isMovingObject(object, parameters)) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return false; } From a0c797e821ea248f28f69d53dee61303afd2aea5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 5 Feb 2024 17:19:09 +0900 Subject: [PATCH 22/48] fix(avoidance): don't use polygon centroid in shiftable ratio calculation (#6285) Signed-off-by: satoshi-ota Co-authored-by: Kotaro Yoshimoto --- .../src/utils.cpp | 36 ++++++++++--------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 81a3e1004f418..206c990e6e547 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -22,6 +22,7 @@ #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include #include #include @@ -485,11 +486,11 @@ bool isObjectOnRoadShoulder( ObjectData & object, const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { - using boost::geometry::return_centroid; 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"); @@ -507,13 +508,9 @@ bool isObjectOnRoadShoulder( // +: object position // o: nearest point on centerline - lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + const auto centerline_pos = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -535,8 +532,9 @@ bool isObjectOnRoadShoulder( } } - const auto center_to_left_boundary = - distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + const auto center_to_left_boundary = distance2d( + to2D(most_left_lanelet.leftBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); return std::make_pair( center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); @@ -547,7 +545,8 @@ bool isObjectOnRoadShoulder( } const auto arc_coordinates = toArcCoordinates( - to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + to2D(object.overhang_lanelet.centerline().basicLineString()), + to2D(toLaneletPoint(object_pose.position)).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -573,8 +572,9 @@ bool isObjectOnRoadShoulder( } } - const auto center_to_right_boundary = - distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + const auto center_to_right_boundary = distance2d( + to2D(most_right_lanelet.rightBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); return std::make_pair( center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); @@ -585,7 +585,8 @@ bool isObjectOnRoadShoulder( } const auto arc_coordinates = toArcCoordinates( - to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + to2D(object.overhang_lanelet.centerline().basicLineString()), + to2D(toLaneletPoint(object_pose.position)).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = @@ -759,6 +760,8 @@ bool isSatisfiedWithVehicleCondition( 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); @@ -769,9 +772,10 @@ bool isSatisfiedWithVehicleCondition( } // check vehicle shift ratio - lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); - const auto on_ego_driving_lane = - within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); + 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 (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { return true; From 8038abf1c7b4e5fbaf6f3a8c984423cebc8d876c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 7 Feb 2024 03:44:51 +0900 Subject: [PATCH 23/48] fix(avoidance): unexpected stop decision in avoidance module (#6320) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/helper.hpp | 3 +- .../src/shift_line_generator.cpp | 30 ++++++++++++++----- 2 files changed, 25 insertions(+), 8 deletions(-) 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 f5b797978b744..0f72c1aefc33b 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 @@ -261,10 +261,11 @@ class AvoidanceHelper bool isComfortable(const AvoidLineArray & shift_lines) const { + const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { return PathShifter::calcJerkFromLatLonDistance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < - getLateralMaxJerkLimit(); + getLateralMaxJerkLimit() + JERK_BUFFER; }); } 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 c20a8a73c955d..d16cfe2e15c8d 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -183,29 +183,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // prepare distance is not enough. unavoidable. - if (remaining_distance < 1e-3) { + if (avoidance_distance < 1e-3) { object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; return std::nullopt; } // calculate lateral jerk. const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed()); + avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. if (required_jerk < helper_->getLateralMaxJerkLimit()) { return std::make_pair(desire_shift_length, avoidance_distance); } + constexpr double LON_DIST_BUFFER = 1e-3; + // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return std::nullopt; + if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } else { + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; + } } // output avoidance path under lateral jerk constraints. const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( - remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); + avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { object.reason = "LessThanExecutionThreshold"; @@ -216,8 +223,17 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift : -1.0 * feasible_relative_shift_length + current_ego_shift; + if ( + avoidance_distance < + helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } + + const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + const auto infeasible = - std::abs(feasible_shift_length - object.overhang_dist) < + std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER < 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); @@ -225,7 +241,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return std::nullopt; } - return std::make_pair(feasible_shift_length, avoidance_distance); + return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance); }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; From de333413ebf706205bf770aaaa62bf8d0bc1b1d4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 7 Feb 2024 09:09:41 +0900 Subject: [PATCH 24/48] fix(avoidance): ghost debug markers due to duplicated marker id (#6330) fix(avoidance): fix duplicated marker id Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/debug.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 7c55c62bd8cc1..fcca9b1a18f49 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -47,7 +47,7 @@ int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid) int32_t ret = 0; for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); + ret <<= sizeof(int8_t) * 8; ret |= uuid.uuid.at(i); } From dfb98e1bff07b7ecbb41b1a174ff2205b199f85d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Jan 2024 23:32:19 +0900 Subject: [PATCH 25/48] feat(avoidance/goal_planner): execute avoidance and pull over simultaneously (#5979) * feat(avoidance/goal_planner): execute avoidance and pull over simultaneously Signed-off-by: kosuke55 Signed-off-by: kosuke55 * use utils Signed-off-by: kosuke55 * fix overlapped Signed-off-by: kosuke55 * reafactor(behavior_path_planner): move isAllowedGoalModification to common Signed-off-by: kosuke55 * fix readme Signed-off-by: kosuke55 * add goal modification condtion to avoidance Signed-off-by: kosuke55 * clean up * revert param Signed-off-by: kosuke55 * fix param Signed-off-by: kosuke55 * move dead line process Signed-off-by: kosuke55 * fix condition Signed-off-by: kosuke55 * fix crop Signed-off-by: kosuke55 * fix crop * fix typos Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../config/avoidance.param.yaml | 3 +- .../data_structs.hpp | 12 +- .../parameter_helper.hpp | 2 + .../src/shift_line_generator.cpp | 97 +++++++++-- .../src/utils.cpp | 20 ++- .../goal_planner_module.hpp | 4 + .../pull_over_planner_base.hpp | 7 + .../shift_pull_over.hpp | 2 + .../util.hpp | 17 +- .../src/goal_planner_module.cpp | 151 ++++++++++++------ .../src/manager.cpp | 6 +- .../src/shift_pull_over.cpp | 84 ++++++++-- .../src/util.cpp | 134 ++++++++++++++-- .../behavior_path_planner_avoidance_design.md | 3 +- .../utils/utils.hpp | 7 + .../src/utils/utils.cpp | 82 ++++++++++ 16 files changed, 531 insertions(+), 100 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 2d38cebd591f5..b300de2236fcf 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -122,7 +122,8 @@ motorcycle: true # [-] pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] 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 86e1608501831..9b5ae3cb4db9e 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 @@ -164,10 +164,14 @@ struct AvoidanceParameters double object_check_backward_distance{0.0}; double object_check_yaw_deviation{0.0}; - // if the distance between object and goal position is less than this parameter, the module ignore - // the object. + // if the distance between object and goal position is less than this parameter, the module do not + // return center line. double object_check_goal_distance{0.0}; + // if the distance between object and return position is less than this parameter, the module do + // not return center line. + double object_check_return_pose_distance{0.0}; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio{0.0}; @@ -462,14 +466,14 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + AvoidOutline(AvoidLine avoid_line, const std::optional return_line) : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } AvoidLine avoid_line{}; - AvoidLine return_line{}; + std::optional return_line{}; AvoidLineArray middle_lines{}; }; 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 b9af50ce76eb5..9f2fdf7ab96d9 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 @@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_check_return_pose_distance = + getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); p.threshold_distance_object_is_on_center = getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = 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 d16cfe2e15c8d..96e22152a2f93 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) AvoidLineArray ret{}; for (const auto & outline : outlines) { ret.push_back(outline.avoid_line); - ret.push_back(outline.return_line); + if (outline.return_line.has_value()) { + ret.push_back(outline.return_line.value()); + } std::for_each( outline.middle_lines.begin(), outline.middle_lines.end(), @@ -357,7 +359,30 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.object_on_right = utils::avoidance::isOnRight(o); } - if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + const bool skip_return_shift = [&]() { + if (!utils::isAllowedGoalModification(data_->route_handler)) { + return false; + } + const auto goal_pose = data_->route_handler->getGoalPose(); + const double goal_longitudinal_distance = + motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const bool is_return_shift_to_goal = + std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < + parameters_->object_check_return_pose_distance; + if (is_return_shift_to_goal) { + return true; + } + const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; + const bool has_object_near_goal = + tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + parameters_->object_check_goal_distance; + return has_object_near_goal; + }(); + + if (skip_return_shift) { + // if the object is close to goal or ego is about to return near GOAL, do not return + outlines.emplace_back(al_avoid, std::nullopt); + } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); } else { o.reason = "InvalidShiftLine"; @@ -653,35 +678,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( auto & next_outline = outlines.at(i); const auto & return_line = last_outline.return_line; - const auto & avoid_line = next_outline.avoid_line; + if (!return_line.has_value()) { + ret.push_back(outlines.at(i)); + break; + } - if (no_conflict(return_line, avoid_line)) { + const auto & avoid_line = next_outline.avoid_line; + if (no_conflict(return_line.value(), avoid_line)) { ret.push_back(outlines.at(i)); continue; } - const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); + const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); continue; } - if (same_side_shift(return_line, avoid_line)) { + if (same_side_shift(return_line.value(), avoid_line)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + if ( + within(return_line.value(), avoid_line.end_idx) && + within(avoid_line, return_line->start_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + if ( + within(return_line.value(), avoid_line.start_idx) && + within(avoid_line, return_line->end_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); @@ -702,7 +735,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( for (auto & outline : ret) { if (outline.middle_lines.empty()) { - const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); + const auto new_line = + outline.return_line.has_value() + ? fill(outline.avoid_line, outline.return_line.value(), generateUUID()) + : outline.avoid_line; outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -717,8 +753,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); - if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); + if ( + outline.return_line.has_value() && + outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -989,6 +1028,20 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); + if (utils::isAllowedGoalModification(data_->route_handler)) { + const auto has_object_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + data_->route_handler->getGoalPose().position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_object_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift"); + return ret; + } + } + const auto exist_unavoidable_object = std::any_of( data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); @@ -1043,6 +1096,21 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( } } + // if last shift line is near the objects, do not add return shift. + if (utils::isAllowedGoalModification(data_->route_handler)) { + const bool has_last_shift_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + last_sl.end.position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_last_shift_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects"); + return ret; + } + } + // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. // Here the return-shift from ego is added for the in case. @@ -1086,8 +1154,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - const auto remaining_distance = std::min( - arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); + const auto remaining_distance = + utils::isAllowedGoalModification(data_->route_handler) + ? data.to_return_point + : std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 206c990e6e547..5114bd39c9383 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -710,11 +710,13 @@ bool isSatisfiedWithCommonCondition( return false; } - if ( - object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > - to_goal_distance) { - object.reason = "TooNearToGoal"; - return false; + if (!utils::isAllowedGoalModification(planner_data->route_handler)) { + if ( + object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > + to_goal_distance) { + object.reason = "TooNearToGoal"; + return false; + } } return true; @@ -1718,7 +1720,9 @@ void fillAdditionalInfoFromLongitudinal( { for (auto & outline : outlines) { fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); - fillAdditionalInfoFromLongitudinal(data, outline.return_line); + if (outline.return_line.has_value()) { + fillAdditionalInfoFromLongitudinal(data, outline.return_line.value()); + } std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { fillAdditionalInfoFromLongitudinal(data, line); @@ -2210,7 +2214,9 @@ double calcDistanceToReturnDeadLine( } // dead line for goal - if (parameters->enable_dead_line_for_goal) { + if ( + !utils::isAllowedGoalModification(planner_data->route_handler) && + parameters->enable_dead_line_for_goal) { if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index e90162c958be5..9fb9c17f1ca70 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -472,6 +472,10 @@ class GoalPlannerModule : public SceneModuleInterface // new turn signal TurnSignalInfo calcTurnSignalInfo() const; + std::optional last_previous_module_output_{}; + bool hasPreviousModulePathShapeChanged() const; + bool hasDeviatedFromLastPreviousModulePath() const; + // timer for generating pull over path candidates in a separate thread void onTimer(); void onFreespaceParkingTimer(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ddd7c93998654..9c51623bbf24f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -119,6 +119,11 @@ class PullOverPlannerBase } virtual ~PullOverPlannerBase() = default; + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + void setPlannerData(const std::shared_ptr planner_data) { planner_data_ = planner_data; @@ -132,6 +137,8 @@ class PullOverPlannerBase vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; + + BehaviorModuleOutput previous_module_output_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index bd19dc2e87de0..892ef7d5dd303 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -43,6 +43,8 @@ class ShiftPullOver : public PullOverPlannerBase protected: PathWithLaneId generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + std::optional cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 791c35f3afca6..a1fd063bd4116 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -52,8 +52,21 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification(const std::shared_ptr & route_handler); -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path); +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_thresh); + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const double extend_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const Pose & extend_pose); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2ea6f1e100e91..894577f51b795 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -136,11 +136,34 @@ void GoalPlannerModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const +{ + if (!last_previous_module_output_) { + return true; + } + + const auto current_path = getPreviousModuleOutput().path; + + // the terminal distance is far + return calcDistance2d( + last_previous_module_output_->path.points.back().point, + current_path.points.back().point) > 0.3; +} + +bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const +{ + if (!last_previous_module_output_) { + return true; + } + return std::abs(motion_utils::calcLateralOffset( + last_previous_module_output_->path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { - // already generated pull over candidate paths - if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -149,16 +172,30 @@ void GoalPlannerModule::onTimer() return; } - if ( - !planner_data_ || - !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!planner_data_ || !utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } - if (getCurrentStatus() == ModuleStatus::IDLE) { + // check if new pull over path candidates are needed to be generated + const bool need_update = std::invoke([&]() { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + return true; + } + if (hasPreviousModulePathShapeChanged()) { + RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + return true; + } + if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { + RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + return true; + } + return false; + }); + if (!need_update) { return; } + const auto previous_module_output = getPreviousModuleOutput(); const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -173,8 +210,9 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { planner->setPlannerData(planner_data_); + planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path && isCrossingPossible(*pull_over_path)) { + if (pull_over_path) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); @@ -188,9 +226,21 @@ void GoalPlannerModule::onTimer() } } }; + + // todo: currently non centerline input path is supported only by shift pull over + const bool is_center_line_input_path = goal_planner_utils::isReferencePath( + previous_module_output.reference_path, previous_module_output.path, 0.1); + RCLCPP_DEBUG( + getLogger(), "the input path of pull over planner is center line: %d", + is_center_line_input_path); + // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } @@ -198,6 +248,10 @@ void GoalPlannerModule::onTimer() } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } planCandidatePaths(planner, goal_candidate); } } @@ -213,7 +267,10 @@ void GoalPlannerModule::onTimer() const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path_candidates(path_candidates); thread_safe_data_.set_closest_start_pose(closest_start_pose); + RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); } + + last_previous_module_output_ = previous_module_output; } void GoalPlannerModule::onFreespaceParkingTimer() @@ -225,7 +282,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } // fixed goal planner do not use freespace planner - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } @@ -354,7 +411,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets current_lanes = + utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); const bool goal_is_in_current_lanes = std::any_of( current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { @@ -384,14 +443,14 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + const double request_length = utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { @@ -402,7 +461,7 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } @@ -585,7 +644,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const { // calculate goal candidates const auto & route_handler = planner_data_->route_handler; - if (goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (utils::isAllowedGoalModification(route_handler)) { return goal_searcher_->search(); } @@ -603,7 +662,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const BehaviorModuleOutput GoalPlannerModule::plan() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(); } @@ -1094,7 +1153,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(); } @@ -1153,11 +1212,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return PathWithLaneId{}; } - // generate reference path - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + // extend previous module path to generate reference path for stop path + const auto reference_path = std::invoke([&]() -> PathWithLaneId { + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; + const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); + const double s_end = s_current + common_parameters.forward_path_length; + return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + }); + const auto extended_prev_path = goal_planner_utils::extendPath( + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible @@ -1165,7 +1228,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); const auto decel_pose = calcLongitudinalOffsetPose( - reference_path.points, closest_goal_candidate.goal_pose.position, + extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1193,7 +1256,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, *stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; @@ -1204,55 +1267,43 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // slow down for turn signal, insert stop point to stop_pose - decelerateForTurnSignal(*stop_pose, reference_path); + auto stop_path = extended_prev_path; + decelerateForTurnSignal(*stop_pose, stop_path); stop_pose_ = *stop_pose; // for debug wall marker // slow down before the search area. if (decel_pose) { - decelerateBeforeSearchStart(*decel_pose, reference_path); - return reference_path; + decelerateBeforeSearchStart(*decel_pose, stop_path); + return stop_path; } - // if already passed the decel pose, set pull_over_velocity to reference_path. + // if already passed the decel pose, set pull_over_velocity to stop_path. const auto min_decel_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, pull_over_velocity); - for (auto & p : reference_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); + for (auto & p : stop_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { continue; } p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); } - return reference_path; + return stop_path; } PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const { - const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & common_parameters = planner_data_->parameters; - - // generate stop reference path - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); - // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return stop_path; + return getPreviousModuleOutput().path; } // set stop point + auto stop_path = getPreviousModuleOutput().path; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { @@ -1881,7 +1932,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1894,6 +1945,14 @@ void GoalPlannerModule::setDebugData() add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } + // Visualize previous module output + add(createPathMarkerArray( + getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); + if (last_previous_module_output_.has_value()) { + add(createPathMarkerArray( + last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); + } + // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 3079361253c31..8608d6cce2c81 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -419,7 +419,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const { // enable AlwaysExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -434,7 +434,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -449,7 +449,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 712da5aa03a4e..b168da61e6239 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -46,10 +46,10 @@ std::optional ShiftPullOver::plan(const Pose & goal_pose) const int shift_sampling_num = parameters_.shift_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; - // get road and shoulder lanes - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_search_length, forward_search_length, + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output_.path, planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, backward_search_length, forward_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { @@ -99,6 +99,31 @@ PathWithLaneId ShiftPullOver::generateReferencePath( return road_lane_reference_path; } +std::optional ShiftPullOver::cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const +{ + // clip previous module path to shift end pose nearest segment index + const size_t shift_end_idx = + motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + std::vector clipped_points{ + prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected shift end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength( + prev_module_path.points, shift_end_idx, shift_end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_prev_module_path = prev_module_path; + clipped_prev_module_path.points = clipped_points; + + return clipped_prev_module_path; +} + std::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -106,33 +131,55 @@ std::optional ShiftPullOver::generatePullOverPath( const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); - // generate road lane reference path to shift end + // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + previous_module_output_.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + return goal_planner_utils::extendPath( + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose); + } else { // case2 + return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } // calculate shift length - const Pose & shift_end_pose_road_lane = - road_lane_reference_path_to_shift_end.points.back().point.pose; + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + tier4_autoware_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) .y; // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( - road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( - road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); - if (!shift_start_pose) return {}; // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(road_lane_reference_path_to_shift_end); + path_shifter.setPath(processed_prev_module_path.value()); ShiftLine shift_line{}; shift_line.start = *shift_start_pose; shift_line.end = shift_end_pose; @@ -140,7 +187,9 @@ std::optional ShiftPullOver::generatePullOverPath( path_shifter.addShiftLine(shift_line); ShiftedPath shifted_path{}; const bool offset_back = true; // offset front side from reference path - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + if (!path_shifter.generate(&shifted_path, offset_back)) { + return {}; + } shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); motion_utils::insertOrientation(shifted_path.path.points, true); @@ -208,8 +257,13 @@ std::optional ShiftPullOver::generatePullOverPath( pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; - pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); + pull_over_path.debug_poses.push_back(goal_pose); + pull_over_path.debug_poses.push_back(shift_end_pose); + pull_over_path.debug_poses.push_back( + road_lane_reference_path_to_shift_end.points.back().point.pose); + pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); // check if the parking path will leave lanes const auto drivable_lanes = @@ -218,8 +272,10 @@ std::optional ShiftPullOver::generatePullOverPath( const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 23e9c53efae7b..1610c76c11d50 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -196,22 +196,138 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification(const std::shared_ptr & route_handler) +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path) { - return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); + double lateral_deviation = 0.0; + for (const auto & target_point : target_path.points) { + const size_t nearest_index = + motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + lateral_deviation = std::max( + lateral_deviation, + std::abs(tier4_autoware_utils::calcLateralDeviation( + reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); + } + return lateral_deviation; +} + +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) +{ + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) +{ + const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + std::vector clipped_points{ + path.points.begin(), path.points.begin() + end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_path = path; + clipped_path.points = clipped_points; + + return clipped_path; +} + +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length) +{ + const auto & points = path.points; + + double sum_length = 0; + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length + seg_length) { + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.begin() + i}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + + // add precise end pose to cropped points + const double remaining_length = forward_length - sum_length; + const Pose precise_end_pose = + calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0); + if (remaining_length < 0.1) { + // if precise_end_pose is too close, replace the last point + cropped_path.points.back().point.pose = precise_end_pose; + } else { + auto precise_end_point = cropped_path.points.back(); + precise_end_point.point.pose = precise_end_pose; + cropped_path.points.push_back(precise_end_point); + } + return cropped_path; + } + sum_length += seg_length; + } + + // if forward_length is too long, return points after target_seg_idx + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.end()}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + return cropped_path; } -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) { - const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const auto shoulder_lanes = route_handler->getShoulderLanelets(); + const auto & target_terminal_pose = target_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); - lanelet::ConstLanelet closest_shoulder_lane{}; - if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { - return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + PathWithLaneId clipped_path = + cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); } - return false; + auto extended_path = target_path; + const auto start_point = + std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { + const bool is_forward = + tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > + 0.0; + const bool is_close = tier4_autoware_utils::calcDistance2d( + p.point.pose.position, target_terminal_pose.position) < 0.1; + return is_forward && !is_close; + }); + std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); + + extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + + return extended_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const double extend_distance = motion_utils::calcSignedArcLength( + reference_path.points, target_path_terminal_idx, extend_pose.position); + + return extendPath(target_path, reference_path, extend_distance); } } // namespace goal_planner_utils diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index c4580bb1e82c3..5fc1105967b15 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.` | object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | | object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 | +| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | | object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index e2c9fd1e332a2..6a04590d6f6d7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -243,6 +243,9 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); + bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); bool isInLaneletWithYawThreshold( @@ -325,6 +328,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route); + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 7c3a5883989fb..a528b2232520a 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1450,6 +1450,70 @@ lanelet::ConstLanelets getExtendedCurrentLanes( return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanesFromPath(path, planner_data); + + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) @@ -1551,4 +1615,22 @@ std::string convertToSnakeCase(const std::string & input_str) } return output_str; } + +bool isAllowedGoalModification(const std::shared_ptr & route_handler) +{ + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); +} + +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } + + return false; +} } // namespace behavior_path_planner::utils From 4c53f8b5235a7d17018e60a5f71ccc45f1da724b Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 26 Jan 2024 16:03:48 +0900 Subject: [PATCH 26/48] fix(start_planner): update drivable area info and enable idle to running state transition (#6172) Update drivable area info and enable idle to running state transition Signed-off-by: kyoichi-sugahara --- .../utils/drivable_area_expansion/static_drivable_area.cpp | 4 ++++ .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 6 +----- 3 files changed, 6 insertions(+), 6 deletions(-) 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 700263851e21a..93ca24c3a6ca5 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 @@ -1862,6 +1862,10 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // drivable margin + combined_drivable_area_info.drivable_margin = + std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); + return combined_drivable_area_info; } diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a143f34ead649..4261f7819da3f 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override; + bool canTransitIdleToRunningState() override { return true; } /** * @brief init member variables. diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index c9fa15d855268..8614f11223dfe 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -338,11 +338,6 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() -{ - return isActivated() && !isWaitingApproval(); -} - BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { @@ -1253,6 +1248,7 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.planner_type == PlannerType::FREESPACE) { + std::cerr << "Freespace planner updated drivable area." << std::endl; const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; From 68b7d275eb1a5861058716ee76fdf19cde792300 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jan 2024 22:57:35 +0900 Subject: [PATCH 27/48] fix(static_drivable_area_expansion): fix bound extraction logic (#6006) * fix(static_drivable_area_expansion): fix bound extraction logic Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): define as anon func Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../static_drivable_area.cpp | 66 ++++++++++--------- 1 file changed, 35 insertions(+), 31 deletions(-) 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 93ca24c3a6ca5..6d038c8624f75 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 @@ -102,6 +102,40 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } + +std::vector extractBoundFromPolygon( + const lanelet::ConstPolygon3d & polygon, const size_t start_idx, const size_t end_idx, + const bool clockwise) +{ + std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(polygon[i]); + + if (i + 1 == polygon.size() && clockwise) { + if (end_idx == 0) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = 0; + ret.push_back(polygon[i]); + continue; + } + + if (i == 0 && !clockwise) { + if (end_idx == polygon.size() - 1) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = polygon.size() - 1; + ret.push_back(polygon[i]); + continue; + } + } + + ret.push_back(polygon[end_idx]); + + return ret; +} } // namespace namespace behavior_path_planner::utils::drivable_area_processing @@ -1242,36 +1276,6 @@ std::vector getBoundWithIntersectionAreas( const std::shared_ptr & route_handler, const std::vector & drivable_lanes, const bool is_left) { - const auto extract_bound_from_polygon = - [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret{}; - for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(polygon[i]); - - if (i + 1 == polygon.size() && clockwise) { - if (end_idx == 0) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = 0; - continue; - } - - if (i == 0 && !clockwise) { - if (end_idx == polygon.size() - 1) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = polygon.size() - 1; - continue; - } - } - - ret.push_back(polygon[end_idx]); - - return ret; - }; - std::vector expanded_bound = original_bound; // expand drivable area by using intersection area. @@ -1317,7 +1321,7 @@ std::vector getBoundWithIntersectionAreas( const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); intersection_bound = - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); + extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); } // Step2. check shared bound point. From b42ec90d2f70f6504ab04b0bf4384c60e56bd033 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Dec 2023 08:01:11 +0900 Subject: [PATCH 28/48] fix(static_drivable_area_expansion): fix bug in drivable bound edge processing logic (#5928) Signed-off-by: satoshi-ota --- .../static_drivable_area.cpp | 43 +++++++++++-------- 1 file changed, 26 insertions(+), 17 deletions(-) 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 6d038c8624f75..f4def8b2be39c 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 @@ -33,23 +33,33 @@ namespace { template size_t findNearestSegmentIndexFromLateralDistance( - const std::vector & points, const geometry_msgs::msg::Point & target_point) + const std::vector & points, const geometry_msgs::msg::Pose & target_point, + const double yaw_threshold) { + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const auto base_yaw = tf2::getYaw(target_point.orientation); + const auto yaw = + normalizeRadian(calcAzimuthAngle(points.at(seg_idx), points.at(seg_idx + 1)) - base_yaw); + if (yaw_threshold < std::abs(yaw)) { + continue; + } const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point.position); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx), target_point); + return calcDistance2d(points.at(seg_idx), target_point.position); } if (segment_length < lon_dist) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx + 1), target_point); + return calcDistance2d(points.at(seg_idx + 1), target_point.position); } - return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -61,7 +71,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return motion_utils::findNearestSegmentIndex(points, target_point); + return motion_utils::findNearestSegmentIndex(points, target_point.position); } bool checkHasSameLane( @@ -832,17 +842,17 @@ void generateDrivableArea( const auto front_pose = cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); const auto left_start_point = calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); const auto right_start_point = calcLongitudinalOffsetStartPoint( right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); + motion_utils::findNearestSegmentIndex(left_bound, left_start_point); const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); + motion_utils::findNearestSegmentIndex(right_bound, right_start_point); // Insert a start point path.left_bound.push_back(left_start_point); @@ -854,18 +864,17 @@ void generateDrivableArea( // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); const auto left_goal_point = calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); const auto right_goal_point = calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); + goal_left_start_idx, motion_utils::findNearestSegmentIndex(left_bound, left_goal_point)); const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); + goal_right_start_idx, motion_utils::findNearestSegmentIndex(right_bound, right_goal_point)); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { From 135b9a667d983448b0f974f1ebf0746e4bdb82e1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 5 Jan 2024 16:05:55 +0900 Subject: [PATCH 29/48] fix(static_drivable_area_expansion): fix drivable bound edge process (#6014) Signed-off-by: satoshi-ota --- .../static_drivable_area.cpp | 45 +++++++++++++++++-- 1 file changed, 41 insertions(+), 4 deletions(-) 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 f4def8b2be39c..1faeb28dacc9f 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 @@ -31,6 +31,42 @@ namespace { +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Point & target_point) +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return calcDistance2d(points.at(seg_idx), target_point); + } + if (segment_length < lon_dist) { + return calcDistance2d(points.at(seg_idx + 1), target_point); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + }(); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; + } + } + + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point); +} + template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, @@ -850,9 +886,9 @@ void generateDrivableArea( const auto right_start_point = calcLongitudinalOffsetStartPoint( right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - motion_utils::findNearestSegmentIndex(left_bound, left_start_point); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); const size_t right_start_idx = - motion_utils::findNearestSegmentIndex(right_bound, right_start_point); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Insert a start point path.left_bound.push_back(left_start_point); @@ -872,9 +908,10 @@ void generateDrivableArea( const auto right_goal_point = calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = std::max( - goal_left_start_idx, motion_utils::findNearestSegmentIndex(left_bound, left_goal_point)); + goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); const size_t right_goal_idx = std::max( - goal_right_start_idx, motion_utils::findNearestSegmentIndex(right_bound, right_goal_point)); + goal_right_start_idx, + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { From 969624e89e5ebb750784f5fcb10632b4569e424f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Jan 2024 09:28:49 +0900 Subject: [PATCH 30/48] feat(avoidance): make it possible to use freespace areas in avoidance module (#6001) * fix(static_drivable_area_expansion): check right/left bound id Signed-off-by: satoshi-ota * feat(static_drivable_area): use freespace area Signed-off-by: satoshi-ota * feat(avoidance): use freespace Signed-off-by: satoshi-ota * fix(AbLC): fix flag Signed-off-by: satoshi-ota * fix(planner_manager): fix flag Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): remove unused arg Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): use lambda Signed-off-by: satoshi-ota * fix(static_drivable_area_expansion): fix invalid access Signed-off-by: satoshi-ota * refactor(static_drivable_area_expansion): improve readability Signed-off-by: satoshi-ota * fix(avoidance): add param Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +- .../config/avoidance.param.yaml | 1 + .../data_structs.hpp | 3 + .../parameter_helper.hpp | 1 + .../src/scene.cpp | 12 +- .../src/planner_manager.cpp | 5 +- .../data_manager.hpp | 1 + .../static_drivable_area.hpp | 28 +- .../static_drivable_area.cpp | 573 ++++++++++++------ 9 files changed, 417 insertions(+), 213 deletions(-) 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 9a4c4c80a256e..fbd8778e79a17 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 @@ -170,11 +170,9 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, true)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, false)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false)); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index b300de2236fcf..d9ae9ed623cb3 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -19,6 +19,7 @@ use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true + use_freespace_areas: true # for debug publish_debug_marker: false 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 9b5ae3cb4db9e..5e162a17a5064 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 @@ -115,6 +115,9 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // use freespace area for avoidance + bool use_freespace_areas{false}; + // consider avoidance return dead line bool enable_dead_line_for_goal{false}; bool enable_dead_line_for_traffic_light{false}; 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 9f2fdf7ab96d9..03902e4328eb2 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 @@ -57,6 +57,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + p.use_freespace_areas = getOrDeclareParameter(*node, ns + "use_freespace_areas"); } // target object diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 4debd2aaad526..5172bcbeb626c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -235,11 +235,13 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, true)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, false)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, false)); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -937,6 +939,8 @@ BehaviorModuleOutput AvoidanceModule::plan() // expand intersection areas current_drivable_area_info.enable_expanding_intersection_areas = parameters_->use_intersection_areas; + // expand freespace areas + current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index e4bfff8fc63b8..fa5a745be9f0e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -216,8 +216,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, - is_driving_forward); + output.path, di.drivable_lanes, false, false, false, data, is_driving_forward); } else { const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); @@ -229,7 +228,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, + di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data, is_driving_forward); } diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index d52906ef4684f..9c879ec08d9fd 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -106,6 +106,7 @@ struct DrivableAreaInfo std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; bool enable_expanding_intersection_areas{false}; + bool enable_expanding_freespace_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 284b4aad20a6a..2e9ad54ae3378 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_path_planner::utils { @@ -40,8 +41,8 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward = true); + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -65,6 +66,11 @@ std::vector expandLanelets( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left); + std::vector getBoundWithHatchedRoadMarkings( const std::vector & original_bound, const std::shared_ptr & route_handler); @@ -75,14 +81,22 @@ std::vector getBoundWithIntersectionAreas( const std::vector & drivable_lanes, const bool is_left); std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool enable_expanding_freespace_areas, const bool is_left, + const bool is_driving_forward = true); + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left); + const bool is_left, const bool is_driving_forward = true); -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left); +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); 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 1faeb28dacc9f..1963ab43355ea 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 @@ -154,6 +154,7 @@ std::vector extractBoundFromPolygon( const bool clockwise) { std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { ret.push_back(polygon[i]); @@ -769,54 +770,27 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward) + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward) { - // extract data - const auto transformed_lanes = utils::transformToLanelets(lanes); - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto route_handler = planner_data->route_handler; - constexpr double overlap_threshold = 0.01; - if (path.points.empty()) { return; } - auto addPoints = - [](const lanelet::ConstLineString3d & points, std::vector & bound) { - for (const auto & bound_p : points) { - const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { - bound.push_back(cp); - } - } - }; - - const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { - for (const auto & transformed_lane : transformed_lanes) { - if (checkHasSameLane(ignore_lanelets, transformed_lane)) { - continue; - } - if (boost::geometry::intersects( - lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + path.left_bound.clear(); + path.right_bound.clear(); // Insert Position - auto left_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, true); - auto right_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, false); - - if (left_bound.empty() || right_bound.empty()) { + path.left_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, true, + is_driving_forward); + path.right_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, false, + is_driving_forward); + + if (path.left_bound.empty() || path.right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, @@ -824,135 +798,10 @@ void generateDrivableArea( return; } - // Insert points after goal - lanelet::ConstLanelet goal_lanelet; - if ( - route_handler->getGoalLanelet(&goal_lanelet) && - checkHasSameLane(transformed_lanes, goal_lanelet)) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); - const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); - const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); - lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; - if (goal_left_lanelet) { - goal_lanelets.push_back(*goal_left_lanelet); - } - if (goal_right_lanelet) { - goal_lanelets.push_back(*goal_right_lanelet); - } - - for (const auto & lane : lanes_after_goal) { - // If lane is already in the transformed lanes, ignore it - if (checkHasSameLane(transformed_lanes, lane)) { - continue; - } - // Check if overlapped - const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) - : has_overlap(lane)); - if (is_overlapped) { - continue; - } - - addPoints(lane.leftBound3d(), left_bound); - addPoints(lane.rightBound3d(), right_bound); - } - } - - if (!is_driving_forward) { - std::reverse(left_bound.begin(), left_bound.end()); - std::reverse(right_bound.begin(), right_bound.end()); - } - - path.left_bound.clear(); - path.right_bound.clear(); - - const auto [left_start_idx, right_start_idx] = [&]() { - const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( - path.points, current_pose.position, current_seg_idx, - planner_data->parameters.forward_path_length, - planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); - - constexpr double front_length = 0.5; - const auto front_pose = - cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; - const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); - const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); - const auto left_start_point = - calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_point = calcLongitudinalOffsetStartPoint( - right_bound, front_pose, front_right_start_idx, -front_length); - const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); - const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); - - // Insert a start point - path.left_bound.push_back(left_start_point); - path.right_bound.push_back(right_start_point); - - return std::make_pair(left_start_idx, right_start_idx); - }(); - - // Get Closest segment for the goal point - const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; - const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); - const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); - const auto left_goal_point = - calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_point = - calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); - const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); - const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); - - // Insert middle points - for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); - if (dist > overlap_threshold) { - path.left_bound.push_back(next_point); - } - } - for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); - if (dist > overlap_threshold) { - path.right_bound.push_back(next_point); - } - } - - // Insert a goal point - if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > - overlap_threshold) { - path.left_bound.push_back(left_goal_point); - } - if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > - overlap_threshold) { - path.right_bound.push_back(right_goal_point); - } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { drivable_area_expansion::expand_drivable_area(path, planner_data); } - - // make bound longitudinally monotonic - // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if ( - is_driving_forward && - (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { - makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound - makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound - } } void generateDrivableArea( @@ -1428,15 +1277,333 @@ std::vector getBoundWithIntersectionAreas( return expanded_bound; } +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left) +{ + using lanelet::utils::to2D; + using lanelet::utils::conversion::toGeomMsgPt; + using lanelet::utils::conversion::toLaneletPoint; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto & route_handler = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return std::make_pair(original_bound, false); + } + + std::sort(polygons.begin(), polygons.end(), [&ego_pose](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + to2D(a).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + const double b_distance = boost::geometry::distance( + to2D(b).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + return a_distance < b_distance; + }); + + const auto & polygon = polygons.front(); + + const auto original_bound_itr = + std::find_if(original_bound.begin(), original_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto original_bound_rev_itr = + std::find_if(original_bound.rbegin(), original_bound.rend(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto other_side_bound_itr = + std::find_if(other_side_bound.begin(), other_side_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + if ( + original_bound_itr == original_bound.end() || other_side_bound_itr == other_side_bound.end()) { + return std::make_pair(original_bound, false); + } + + const auto footprint = planner_data->parameters.vehicle_info.createFootprint(); + const auto vehicle_polygon = transformVector(footprint, pose2transform(ego_pose)); + const auto is_driving_freespace = + !boost::geometry::disjoint(vehicle_polygon, to2D(polygon).basicPolygon()); + + const auto is_clockwise_polygon = boost::geometry::is_valid(to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + + const auto extract_bound_from_polygon = [&polygon, &is_clockwise_iteration]( + const auto & start_id, const auto & end_id) { + const auto start_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == start_id; }); + + const auto end_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == end_id; }); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_point_itr); + const size_t end_idx = std::distance(polygon.begin(), end_point_itr); + + return extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); + }; + + const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left]( + const auto & bound, const auto trim_behind_bound) { + if (!is_driving_freespace) { + return bound; + } + + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); + + std::vector ret; + for (size_t i = 1; i < bound.size(); ++i) { + const auto intersect = tier4_autoware_utils::intersect( + ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), + toGeomMsgPt(bound.at(i))); + + ret.push_back(bound.at(i - 1)); + + if (intersect.has_value()) { + ret.emplace_back( + lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z); + break; + } + } + + return ret; + }; + + std::vector expanded_bound; + + enum class RouteCase { + ROUTE_IS_PASS_THROUGH_FREESPACE = 0, + GOAL_POS_IS_IN_FREESPACE, + INIT_POS_IS_IN_FREESPACE, + OTHER, + }; + + const auto route_case = [&]() { + if (original_bound_itr->id() != original_bound_rev_itr->id()) { + return RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.front().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::INIT_POS_IS_IN_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.back().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::GOAL_POS_IS_IN_FREESPACE; + } + return RouteCase::OTHER; + }(); + + switch (route_case) { + case RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), original_bound_rev_itr->id()); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), polygon_bound.begin(), polygon_bound.end()); + expanded_bound.insert( + expanded_bound.end(), std::next(original_bound_rev_itr).base(), original_bound.end()); + break; + } + case RouteCase::INIT_POS_IS_IN_FREESPACE: { + auto polygon_bound = + extract_bound_from_polygon(other_side_bound_itr->id(), original_bound_itr->id()); + std::reverse(polygon_bound.begin(), polygon_bound.end()); + auto bound_edge = get_bound_edge(polygon_bound, true); + std::reverse(bound_edge.begin(), bound_edge.end()); + + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + expanded_bound.insert(expanded_bound.end(), original_bound_itr, original_bound.end()); + break; + } + case RouteCase::GOAL_POS_IS_IN_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), other_side_bound_itr->id()); + const auto bound_edge = get_bound_edge(polygon_bound, false); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + break; + } + case RouteCase::OTHER: { + expanded_bound = original_bound; + break; + } + default: + throw std::domain_error("invalid case."); + } + + const auto goal_is_in_freespace = boost::geometry::within( + to2D(toLaneletPoint(route_handler->getGoalPose().position).basicPoint()), + to2D(polygon).basicPolygon()); + + return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace); +} + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left, const bool is_driving_forward) +{ + const auto lanelets = utils::transformToLanelets(drivable_lanes); + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; + const auto & vehicle_length = planner_data->parameters.vehicle_length; + constexpr double overlap_threshold = 0.01; + + const auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } + } + }; + + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { + for (const auto & transformed_lane : lanelets) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + + std::vector processed_bound; + std::vector tmp_bound = original_bound; + + // Insert points after goal + lanelet::ConstLanelet goal_lanelet; + if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet.has_value()) { + goal_lanelets.push_back(goal_left_lanelet.value()); + } + if (goal_right_lanelet.has_value()) { + goal_lanelets.push_back(goal_right_lanelet.value()); + } + + for (const auto & lane : lanes_after_goal) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(lanelets, lane)) { + continue; + } + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); + if (is_overlapped) { + continue; + } + + if (is_left) { + addPoints(lane.leftBound3d(), tmp_bound); + } else { + addPoints(lane.rightBound3d(), tmp_bound); + } + } + } + + if (!is_driving_forward) { + std::reverse(tmp_bound.begin(), tmp_bound.end()); + } + + const auto start_idx = [&]() { + const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); + const auto cropped_path_points = motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, + planner_data->parameters.forward_path_length, + planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); + + constexpr double front_length = 0.5; + const auto front_pose = + cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; + const size_t front_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2); + const auto start_point = + calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length); + + // Insert a start point + processed_bound.push_back(start_point); + + return findNearestSegmentIndexFromLateralDistance(tmp_bound, start_point); + }(); + + // Get Closest segment for the goal point + const auto [goal_idx, goal_point] = [&]() { + const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; + const size_t goal_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2); + const auto goal_point = + calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length); + const size_t goal_idx = + std::max(goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_point)); + + return std::make_pair(goal_idx, goal_point); + }(); + + // Insert middle points + for (size_t i = start_idx + 1; i <= goal_idx; ++i) { + const auto & next_point = tmp_bound.at(i); + const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + if (dist > overlap_threshold) { + processed_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + processed_bound.push_back(goal_point); + } + + const bool skip_monotonic_process = + !is_driving_forward || + !(enable_expanding_hatched_road_markings || enable_expanding_intersection_areas); + + return skip_monotonic_process + ? processed_bound + : makeBoundLongitudinallyMonotonic(processed_bound, path, planner_data, is_left); +} + // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) + const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { + using motion_utils::removeOverlapPoints; + + const auto & route_handler = planner_data->route_handler; + // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { + const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) { constexpr double overlap_threshold = 0.01; std::vector points; @@ -1463,20 +1630,35 @@ std::vector calcBound( }; // Step1. create drivable bound from drivable lanes. - std::vector bound_points = convert_to_points(drivable_lanes); + auto [bound_points, skip_post_process] = [&]() { + if (!enable_expanding_freespace_areas) { + return std::make_pair(convert_to_points(drivable_lanes, is_left), false); + } + return getBoundWithFreeSpaceAreas( + convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left), + planner_data, is_left); + }(); + + const auto post_process = [&](const auto & bound, const auto skip) { + return skip + ? bound + : postProcess( + bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, is_left, is_driving_forward); + }; // Step2. if there is no drivable area defined by polygon, return original drivable bound. if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } - // Step3. if there are hatched road markings, expand drivable bound with the polygon. + // Step3.if there are hatched road markings, expand drivable bound with the polygon. if (enable_expanding_hatched_road_markings) { bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); } if (!enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } // Step4. if there are intersection areas, expand drivable bound with the polygon. @@ -1485,12 +1667,12 @@ std::vector calcBound( getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left) +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left) { using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcAzimuthAngle; @@ -1536,7 +1718,7 @@ void makeBoundLongitudinallyMonotonic( const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { auto ret = bound_with_pose; - const double offset = is_bound_left ? 100.0 : -100.0; + const double offset = is_left ? 100.0 : -100.0; size_t start_bound_idx = 0; @@ -1616,14 +1798,14 @@ void makeBoundLongitudinallyMonotonic( // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is // opposite. - const double lat_offset = is_bound_left ? 100.0 : -100.0; + const double lat_offset = is_left ? 100.0 : -100.0; const auto p_bound_1 = getPose(bound_with_pose.at(bound_idx)); const auto p_bound_2 = getPose(bound_with_pose.at(bound_idx + 1)); const auto p_bound_offset = calcOffsetPose(p_bound_1, 0.0, lat_offset, 0.0); - if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_bound_left, is_reverse)) { + if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_left, is_reverse)) { bound_idx++; continue; } @@ -1709,18 +1891,16 @@ void makeBoundLongitudinallyMonotonic( return ret; }; - const auto original_bound = is_bound_left ? path.left_bound : path.right_bound; - if (path.points.empty()) { - return; + return original_bound; } - if (original_bound.empty()) { - return; + if (original_bound.size() < 2) { + return original_bound; } if (path.points.front().lane_ids.empty()) { - return; + return original_bound; } // step.1 create bound with pose vector. @@ -1762,14 +1942,14 @@ void makeBoundLongitudinallyMonotonic( p.forward_path_length, p); if (centerline_path.points.size() < 2) { - return; + return original_bound; } const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); if (ego_idx >= end_idx) { - return; + return original_bound; } clipped_points.insert( @@ -1778,7 +1958,7 @@ void makeBoundLongitudinallyMonotonic( } if (clipped_points.empty()) { - return; + return original_bound; } // step.3 update bound pose by reference path pose. @@ -1799,11 +1979,7 @@ void makeBoundLongitudinallyMonotonic( auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); // step.7 remove sharp bound points. - if (is_bound_left) { - path.left_bound = remove_sharp_points(full_monotonic_bound); - } else { - path.right_bound = remove_sharp_points(full_monotonic_bound); - } + return remove_sharp_points(full_monotonic_bound); } std::vector combineDrivableLanes( @@ -1813,7 +1989,9 @@ std::vector combineDrivableLanes( const auto has_same_lane = [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { - return ll.id() == target_lane.id(); + return ll.id() == target_lane.id() && + ll.rightBound3d().id() == target_lane.rightBound3d().id() && + ll.leftBound3d().id() == target_lane.leftBound3d().id(); }) != lanes.end(); }; @@ -1912,6 +2090,11 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // enable expanding freespace areas + combined_drivable_area_info.enable_expanding_freespace_areas = + drivable_area_info1.enable_expanding_freespace_areas || + drivable_area_info2.enable_expanding_freespace_areas; + // drivable margin combined_drivable_area_info.drivable_margin = std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); From c74a53b7d128b4015df3fd3c728430cef5be70e2 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 8 Feb 2024 09:06:17 +0900 Subject: [PATCH 31/48] feat(avoidance): consider objects on shift side lane (#6252) * fix(avoidance): check safety for only moving objects Signed-off-by: satoshi-ota * feat(avoidance): consider ignore object in avoid margin calculation Signed-off-by: satoshi-ota * fix(avoidance): use nearest overhang point Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 22 +-- .../data_structs.hpp | 23 +-- .../behavior_path_avoidance_module/helper.hpp | 3 +- .../behavior_path_avoidance_module/utils.hpp | 8 +- .../src/debug.cpp | 12 +- .../src/scene.cpp | 33 ++-- .../src/shift_line_generator.cpp | 2 +- .../src/utils.cpp | 154 ++++++++++-------- 8 files changed, 138 insertions(+), 119 deletions(-) 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 fbd8778e79a17..110586aab8c16 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 @@ -30,16 +30,6 @@ namespace behavior_path_planner { -namespace -{ -lanelet::BasicLineString3d toLineString3d(const std::vector & bound) -{ - lanelet::BasicLineString3d ret{}; - std::for_each( - bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); - return ret; -} -} // namespace AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) @@ -169,10 +159,10 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( // calc drivable bound const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); - data.left_bound = toLineString3d(utils::calcBound( - data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true)); - data.right_bound = toLineString3d(utils::calcBound( - data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false)); + data.left_bound = utils::calcBound( + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true); + data.right_bound = utils::calcBound( + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects @@ -272,8 +262,8 @@ ObjectData AvoidanceByLaneChange::createObjectData( : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, data.reference_path, object_data.overhang_pose.position); + object_data.overhang_points = + utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; 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 5e162a17a5064..a536fa1b568cb 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 @@ -336,12 +336,8 @@ struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang) - : object(std::move(obj)), - to_centerline(lat), - longitudinal(lon), - length(len), - overhang_dist(overhang) + ObjectData(PredictedObject obj, double lat, double lon, double len) + : object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len) { } @@ -365,9 +361,6 @@ struct ObjectData // avoidance target // longitudinal length of vehicle, in Frenet coordinate double length{0.0}; - // lateral distance to the closest footprint, in Frenet coordinate - double overhang_dist{0.0}; - // lateral shiftable ratio double shiftable_ratio{0.0}; @@ -392,9 +385,6 @@ struct ObjectData // avoidance target // the position at the detected moment Pose init_pose; - // the position of the overhang - Pose overhang_pose; - // envelope polygon Polygon2d envelope_poly{}; @@ -425,6 +415,9 @@ struct ObjectData // avoidance target // object direction. Direction direction{Direction::NONE}; + // overhang points (sort by distance) + std::vector> overhang_points{}; + // unavoidable reason std::string reason{}; @@ -432,7 +425,7 @@ struct ObjectData // avoidance target std::optional avoid_margin{std::nullopt}; // the nearest bound point (use in road shoulder distance calculation) - std::optional nearest_bound_point{std::nullopt}; + std::optional> narrowest_place{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -541,9 +534,9 @@ struct AvoidancePlanningData std::vector drivable_lanes{}; - lanelet::BasicLineString3d right_bound{}; + std::vector right_bound{}; - lanelet::BasicLineString3d left_bound{}; + std::vector left_bound{}; bool safe{false}; 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 0f72c1aefc33b..cc00bfbb978f5 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 @@ -176,7 +176,8 @@ class AvoidanceHelper { using utils::avoidance::calcShiftLength; - const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin); + const auto shift_length = + calcShiftLength(is_on_right, object.overhang_points.front().first, margin); return is_on_right ? std::min(shift_length, getLeftShiftBound()) : std::max(shift_length, getRightShiftBound()); } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index cbf68ada44abb..afde14875030f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -56,8 +56,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al); void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); -double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); +std::vector> calcEnvelopeOverhangDistance( + const ObjectData & object_data, const PathWithLaneId & path); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, @@ -127,6 +127,10 @@ void filterTargetObjects( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index fcca9b1a18f49..cc7870375d0db 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -113,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: MarkerArray msg; for (const auto & object : objects) { - if (!object.nearest_bound_point.has_value()) { + if (!object.narrowest_place.has_value()) { continue; } @@ -122,8 +122,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999)); - marker.points.push_back(object.overhang_pose.position); - marker.points.push_back(object.nearest_bound_point.value()); + marker.points.push_back(object.narrowest_place.value().first); + marker.points.push_back(object.narrowest_place.value().second); marker.id = uuidToInt32(object.object.object_id); msg.markers.push_back(marker); } @@ -133,7 +133,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); - marker.pose.position = object.nearest_bound_point.value(); + marker.pose.position = object.narrowest_place.value().second; std::ostringstream string_stream; string_stream << object.to_road_shoulder_distance << "[m]"; marker.text = string_stream.str(); @@ -469,7 +469,7 @@ MarkerArray createDrivableBounds( createMarkerColor(r, g, b, 0.999)); for (const auto & p : data.right_bound) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(p); } msg.markers.push_back(marker); @@ -482,7 +482,7 @@ MarkerArray createDrivableBounds( createMarkerColor(r, g, b, 0.999)); for (const auto & p : data.left_bound) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(p); } msg.markers.push_back(marker); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 5172bcbeb626c..685739fee432e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -234,14 +234,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // calc drivable bound auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); - data.left_bound = toLineString3d(utils::calcBound( + data.left_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, - parameters_->use_freespace_areas, true)); - data.right_bound = toLineString3d(utils::calcBound( + parameters_->use_freespace_areas, true); + data.right_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, - parameters_->use_freespace_areas, false)); + parameters_->use_freespace_areas, false); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -294,6 +294,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; + using utils::avoidance::updateRoadShoulderDistance; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. constexpr double MARGIN = 10.0; @@ -315,6 +316,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // Filter out the objects to determine the ones to be avoided. filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); + updateRoadShoulderDistance(data, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -929,10 +931,6 @@ BehaviorModuleOutput AvoidanceModule::plan() DrivableAreaInfo current_drivable_area_info; // generate drivable lanes current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; @@ -941,6 +939,21 @@ BehaviorModuleOutput AvoidanceModule::plan() parameters_->use_intersection_areas; // expand freespace areas current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; + // generate obstacle polygons + if (parameters_->enable_bound_clipping) { + ObjectDataArray clip_objects; + // If avoidance is executed by both behavior and motion, only non-avoidable object will be + // extracted from the drivable area. + std::for_each( + data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) clip_objects.push_back(object); + }); + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + } else { + current_drivable_area_info.obstacles.clear(); + } output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); @@ -1150,8 +1163,8 @@ bool AvoidanceModule::isValidShiftLine( const size_t end_idx = shift_lines.back().end_idx; const auto path = shifter_for_validate.getReferencePath(); - const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound); - const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); for (size_t i = start_idx; i <= end_idx; ++i) { const auto p = getPoint(path.points.at(i)); lanelet::BasicPoint2d basic_point{p.x, p.y}; 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 96e22152a2f93..cd2a08489fd29 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -235,7 +235,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; const auto infeasible = - std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER < + std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER < 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 5114bd39c9383..818b25a0d2387 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -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/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" @@ -823,8 +824,8 @@ bool isNoNeedAvoidanceBehavior( return false; } - const auto shift_length = - calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value()); + const auto shift_length = calcShiftLength( + isOnRight(object), object.overhang_points.front().first, object.avoid_margin.value()); if (!isShiftNecessary(isOnRight(object), shift_length)) { object.reason = "NotNeedAvoidance"; return true; @@ -885,45 +886,41 @@ double getRoadShoulderDistance( return 0.0; } - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto & p1_object = object.overhang_pose.position; - const auto p_tmp = - geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); - const auto p2_object = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; - - // TODO(Satoshi OTA): check if the basic point is on right or left of bound. - const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; - - std::vector intersects; - for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); - - const auto opt_intersect = - tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); - - if (!opt_intersect) { - continue; - } + std::vector> intersects; + for (const auto & p1 : object.overhang_points) { + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto p_tmp = + geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); + const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + for (size_t i = 1; i < bound.size(); i++) { + const auto opt_intersect = + tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); - intersects.push_back(opt_intersect.value()); + if (!opt_intersect) { + continue; + } + + intersects.emplace_back(p1.second, opt_intersect.value()); + } } + std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) { + return calcDistance2d(a.first, a.second) < calcDistance2d(b.first, b.second); + }); + if (intersects.empty()) { return 0.0; } - std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { - return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); - }); - - object.nearest_bound_point = intersects.front(); + object.narrowest_place = intersects.front(); - return calcDistance2d(p1_object, object.nearest_bound_point.value()); + return calcDistance2d( + object.narrowest_place.value().first, object.narrowest_place.value().second); } } // namespace filtering_utils @@ -1079,34 +1076,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( return; } -double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) +std::vector> calcEnvelopeOverhangDistance( + const ObjectData & object_data, const PathWithLaneId & path) { - double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number + std::vector> overhang_points{}; for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. const auto idx = findNearestIndex(path.points, point); const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); - - const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { - if (lateral > largest_overhang) { - overhang_pose = point; - } - return std::max(largest_overhang, lateral); - }; - - const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() { - if (lateral < largest_overhang) { - overhang_pose = point; - } - return std::min(largest_overhang, lateral); - }; - - largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left(); + overhang_points.emplace_back(lateral, point); } - return largest_overhang; + std::sort(overhang_points.begin(), overhang_points.end(), [&](const auto & a, const auto & b) { + return isOnRight(object_data) ? b.first < a.first : a.first < b.first; + }); + return overhang_points; } void setEndData( @@ -1189,24 +1174,21 @@ std::vector generateObstaclePolygonsForDrivableArea( { std::vector obstacles_for_drivable_area; - if (objects.empty() || !parameters->enable_bound_clipping) { + if (objects.empty()) { return obstacles_for_drivable_area; } for (const auto & object : objects) { - // If avoidance is executed by both behavior and motion, only non-avoidable object will be - // extracted from the drivable area. - if (!parameters->disable_path_update) { - if (object.is_avoidable) { - continue; - } - } - // check if avoid marin is calculated if (!object.avoid_margin.has_value()) { continue; } + // check original polygon + if (object.envelope_poly.outer().empty()) { + continue; + } + const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); @@ -1454,10 +1436,10 @@ void fillAvoidanceNecessity( 0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor; const auto check_necessity = [&](const auto hysteresis_factor) { - return (isOnRight(object_data) && - std::abs(object_data.overhang_dist) < safety_margin * hysteresis_factor) || + return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) < + safety_margin * hysteresis_factor) || (!isOnRight(object_data) && - object_data.overhang_dist < safety_margin * hysteresis_factor); + object_data.overhang_points.front().first < safety_margin * hysteresis_factor); }; const auto id = object_data.object.object_id; @@ -1620,6 +1602,40 @@ void compensateDetectionLost( } } +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + ObjectDataArray clip_objects; + std::for_each(data.other_objects.begin(), data.other_objects.end(), [&](const auto & object) { + if (!filtering_utils::isMovingObject(object, parameters)) { + clip_objects.push_back(object); + } + }); + for (auto & o : clip_objects) { + 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); + + o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + } + const auto extract_obstacles = generateObstaclePolygonsForDrivableArea( + clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0); + + auto tmp_path = data.reference_path; + tmp_path.left_bound = data.left_bound; + tmp_path.right_bound = data.right_bound; + utils::extractObstaclesFromDrivableArea(tmp_path, extract_obstacles); + + data.left_bound = tmp_path.left_bound; + data.right_bound = tmp_path.right_bound; + + for (auto & o : data.target_objects) { + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + } +} + void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, @@ -1643,8 +1659,7 @@ void filterTargetObjects( } // Find the footprint point closest to the path, set to object_data.overhang_distance. - o.overhang_dist = - calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position); + 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); @@ -1851,7 +1866,10 @@ std::vector getSafetyCheckTargetObjects( PredictedObjects ret{}; std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { - ret.objects.push_back(object.object); + // check only moving objects + if (filtering_utils::isMovingObject(object, parameters)) { + ret.objects.push_back(object.object); + } } }); return ret; From e2aa8af7aea3cffc381ff07d48cc7c438ebbb111 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Feb 2024 09:54:01 +0900 Subject: [PATCH 32/48] fix(avoidance): fix bug in minimum overhang distance calculation (#6372) Signed-off-by: satoshi-ota --- .../src/utils.cpp | 41 +++++++++++++------ 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 818b25a0d2387..645471c4f9d00 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -886,41 +886,58 @@ double getRoadShoulderDistance( return 0.0; } - std::vector> intersects; + std::vector> intersects; for (const auto & p1 : object.overhang_points) { const auto centerline_pose = lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); const auto p_tmp = geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); - const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; // TODO(Satoshi OTA): check if the basic point is on right or left of bound. const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; for (size_t i = 1; i < bound.size(); i++) { - const auto opt_intersect = - tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); - - if (!opt_intersect) { - continue; + { + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + const auto opt_intersect = + tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + + if (opt_intersect.has_value()) { + intersects.emplace_back( + calcDistance2d(p1.second, opt_intersect.value()), p1.second, opt_intersect.value()); + break; + } } - intersects.emplace_back(p1.second, opt_intersect.value()); + { + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -100.0 : 100.0), 0.0).position; + const auto opt_intersect = + tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + + if (opt_intersect.has_value()) { + intersects.emplace_back( + -1.0 * calcDistance2d(p1.second, opt_intersect.value()), p1.second, + opt_intersect.value()); + break; + } + } } } std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) { - return calcDistance2d(a.first, a.second) < calcDistance2d(b.first, b.second); + return std::get<0>(a) < std::get<0>(b); }); if (intersects.empty()) { return 0.0; } - object.narrowest_place = intersects.front(); + object.narrowest_place = + std::make_pair(std::get<1>(intersects.front()), std::get<2>(intersects.front())); - return calcDistance2d( - object.narrowest_place.value().first, object.narrowest_place.value().second); + return std::get<0>(intersects.front()); } } // namespace filtering_utils From e7a9d7b14e2c183b86d1fab4343b8d1cdccf6d6a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Feb 2024 12:43:57 +0900 Subject: [PATCH 33/48] chore(avoidance): add maintainer (#6387) Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml index f72adf0591493..fc34172bb2ce9 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/behavior_path_avoidance_module/package.xml @@ -6,6 +6,7 @@ The behavior_path_avoidance_module package Takamasa Horibe + Zulfaqar Azmi Satoshi Ota Fumiya Watanabe Kyoichi Sugahara From cadd516d4ed1f9e776dc620de2cf61347a1ac598 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Feb 2024 15:39:36 +0900 Subject: [PATCH 34/48] fix(avoidance): safety check target lanes (#6324) * fix(avoidance): improve safety check lanes Signed-off-by: satoshi-ota * fix(avoidance): check both side lane if need Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/utils.hpp | 5 +- .../src/scene.cpp | 19 +++++-- .../src/utils.cpp | 55 +++++++++++++------ 3 files changed, 56 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index afde14875030f..da3a224689149 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -78,6 +78,7 @@ std::vector generateObstaclePolygonsForDrivableArea( const double vehicle_width); lanelet::ConstLanelets getAdjacentLane( + const lanelet::ConstLanelet & current_lane, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); @@ -147,8 +148,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift, - DebugData & debug); + const std::shared_ptr & parameters, const bool has_left_shift, + const bool has_right_shift, DebugData & debug); std::pair separateObjectsByPath( const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 685739fee432e..b8253d9aabc0e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -695,30 +695,39 @@ bool AvoidanceModule::isSafePath( } const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - const auto is_right_shift = [&]() -> std::optional { + + const auto has_left_shift = [&]() { for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); if (parameters_->lateral_avoid_check_threshold < length) { - return false; + return true; } + } + + return false; + }(); + + const auto has_right_shift = [&]() { + for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { + const auto length = shifted_path.shift_length.at(i); if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { return true; } } - return std::nullopt; + return false; }(); - if (!is_right_shift.has_value()) { + if (!has_left_shift && !has_right_shift) { return true; } const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); + avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); if (safety_check_target_objects.empty()) { return true; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 645471c4f9d00..89d993ab6d349 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1264,11 +1264,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath( throw std::logic_error("empty path."); } - if (path.points.front().lane_ids.empty()) { + const auto idx = planner_data->findEgoIndex(path.points); + + if (path.points.at(idx).lane_ids.empty()) { throw std::logic_error("empty lane ids."); } - const auto start_id = path.points.front().lane_ids.front(); + const auto start_id = path.points.at(idx).lane_ids.front(); const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id); const auto & p = planner_data->parameters; @@ -1815,6 +1817,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( } lanelet::ConstLanelets getAdjacentLane( + const lanelet::ConstLanelet & current_lane, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) { @@ -1823,18 +1826,17 @@ lanelet::ConstLanelets getAdjacentLane( const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - const auto ego_succeeding_lanes = rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); lanelet::ConstLanelets lanes{}; + + const auto exist = [&lanes](const auto id) { + const auto itr = std::find_if( + lanes.begin(), lanes.end(), [&id](const auto & lane) { return lane.id() == id; }); + return itr != lanes.end(); + }; + for (const auto & lane : ego_succeeding_lanes) { const auto opt_left_lane = rh->getLeftLanelet(lane, true, false); if (!is_right_shift && opt_left_lane) { @@ -1849,6 +1851,20 @@ lanelet::ConstLanelets getAdjacentLane( const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); if (is_right_shift && !right_opposite_lanes.empty()) { lanes.push_back(right_opposite_lanes.front()); + + for (const auto & prev_lane : rh->getPreviousLanelets(right_opposite_lanes.front())) { + if (!exist(prev_lane.id())) { + lanes.push_back(prev_lane); + } + } + } + } + + for (const auto & lane : lanes) { + for (const auto & next_lane : rh->getNextLanelets(lane)) { + if (!exist(next_lane.id())) { + lanes.push_back(next_lane); + } } } @@ -1857,14 +1873,14 @@ lanelet::ConstLanelets getAdjacentLane( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift, - DebugData & debug) + const std::shared_ptr & parameters, const bool has_left_shift, + const bool has_right_shift, DebugData & debug) { const auto & p = parameters; const auto check_right_lanes = - (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + (has_right_shift && p->check_shift_side_lane) || (has_left_shift && p->check_other_side_lane); const auto check_left_lanes = - (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + (has_left_shift && p->check_shift_side_lane) || (has_right_shift && p->check_other_side_lane); std::vector target_objects; @@ -1902,9 +1918,16 @@ std::vector getSafetyCheckTargetObjects( return ret; }(); + lanelet::ConstLanelet closest_lanelet; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + if (!lanelet::utils::query::getClosestLanelet( + data.current_lanelets, ego_pose, &closest_lanelet)) { + return {}; + } + // check right lanes if (check_right_lanes) { - const auto check_lanes = getAdjacentLane(planner_data, p, true); + const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -1926,7 +1949,7 @@ std::vector getSafetyCheckTargetObjects( // check left lanes if (check_left_lanes) { - const auto check_lanes = getAdjacentLane(planner_data, p, false); + const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, false); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( From a9f0bbd9793e658191e4621f848389e4e7ade6e0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 15 Feb 2024 20:35:47 +0900 Subject: [PATCH 35/48] fix(avoidance): limit drivable lane only when the ego in on original lane (#6349) Signed-off-by: satoshi-ota --- .../src/scene.cpp | 4 +- .../behavior_path_avoidance_module/utils.hpp | 9 ++- .../src/scene.cpp | 48 ++++++++++++--- .../src/utils.cpp | 61 +++++++++++++++---- 4 files changed, 97 insertions(+), 25 deletions(-) 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 110586aab8c16..2b2e0e36507c5 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 @@ -152,8 +152,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( // expand drivable lanes std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( - lanelet, planner_data_, avoidance_parameters_, false)); + data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, avoidance_parameters_)); }); // calc drivable bound diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index da3a224689149..02d8d68cf7a56 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj); double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); +bool isWithinLanes( + const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data); + bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length); @@ -157,9 +160,11 @@ std::pair separateObjectsByPath( const std::shared_ptr & parameters, const double object_check_forward_distance, DebugData & debug); -DrivableLanes generateExpandDrivableLanes( +DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet); + +DrivableLanes generateExpandedDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool in_avoidance_maneuver); + const std::shared_ptr & parameters); double calcDistanceToReturnDeadLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b8253d9aabc0e..75529c35c2af5 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -32,8 +33,6 @@ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" #include -#include - #include #include #include @@ -223,24 +222,51 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes - const auto has_shift_point = !path_shifter_.getShiftLines().empty(); - const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); + const auto is_within_current_lane = + utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_); + const auto red_signal_lane_itr = std::find_if( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); + return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_); + }); + const auto not_use_adjacent_lane = + is_within_current_lane && red_signal_lane_itr != data.current_lanelets.end(); + std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( - lanelet, planner_data_, parameters_, in_avoidance_maneuver)); + if (!not_use_adjacent_lane) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + } else if (red_signal_lane_itr->id() != lanelet.id()) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + } else { + data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet)); + } }); // calc drivable bound auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); + const auto use_left_side_hatched_road_marking_area = [&]() { + if (!not_use_adjacent_lane) { + return true; + } + return !planner_data_->route_handler->getRoutingGraphPtr()->left(*red_signal_lane_itr); + }(); + const auto use_right_side_hatched_road_marking_area = [&]() { + if (!not_use_adjacent_lane) { + return true; + } + return !planner_data_->route_handler->getRoutingGraphPtr()->right(*red_signal_lane_itr); + }(); data.left_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, - parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + use_left_side_hatched_road_marking_area, parameters_->use_intersection_areas, parameters_->use_freespace_areas, true); data.right_bound = utils::calcBound( getPreviousModuleOutput().path, planner_data_, shorten_lanes, - parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + use_right_side_hatched_road_marking_area, parameters_->use_intersection_areas, parameters_->use_freespace_areas, false); // reference path @@ -939,7 +965,11 @@ BehaviorModuleOutput AvoidanceModule::plan() { DrivableAreaInfo current_drivable_area_info; // generate drivable lanes - current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + current_drivable_area_info.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + }); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 89d993ab6d349..a0a8e375118f0 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -958,6 +958,44 @@ double calcShiftLength( return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0; } +bool isWithinLanes( + const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data) +{ + const auto & rh = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto transform = tier4_autoware_utils::pose2transform(ego_pose); + const auto footprint = tier4_autoware_utils::transformVector( + planner_data->parameters.vehicle_info.createFootprint(), transform); + + lanelet::ConstLanelet closest_lanelet{}; + if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) { + return true; + } + + lanelet::ConstLanelets concat_lanelets{}; + + // push previous lanelet + lanelet::ConstLanelets prev_lanelet; + if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) { + concat_lanelets.push_back(prev_lanelet.front()); + } + + // push nearest lanelet + { + concat_lanelets.push_back(closest_lanelet); + } + + // push next lanelet + lanelet::ConstLanelet next_lanelet; + if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) { + concat_lanelets.push_back(next_lanelet); + } + + const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets); + + return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon()); +} + bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length) { /** @@ -2077,9 +2115,18 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } -DrivableLanes generateExpandDrivableLanes( +DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet) +{ + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + return current_drivable_lanes; +} + +DrivableLanes generateExpandedDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool in_avoidance_maneuver) + const std::shared_ptr & parameters) { const auto & route_handler = planner_data->route_handler; @@ -2093,11 +2140,6 @@ DrivableLanes generateExpandDrivableLanes( // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto next_lanes = route_handler->getNextLanelets(target_lane); - const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); - if (is_stop_signal && !in_avoidance_maneuver) { - return; - } const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_left_lanelets.empty()) { @@ -2108,11 +2150,6 @@ DrivableLanes generateExpandDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto next_lanes = route_handler->getNextLanelets(target_lane); - const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); - if (is_stop_signal && !in_avoidance_maneuver) { - return; - } const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_right_lanelets.empty()) { From 45035d3697e814046918874e3d3736d69ed7bd12 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 16 Feb 2024 18:01:40 +0900 Subject: [PATCH 36/48] feat(safety_check): add option to use polygon along path in safety check (#6336) * feat(safety_check): add new function to create polygon along path Signed-off-by: satoshi-ota * feat(avoidance): use free steer policy for safety check Signed-off-by: satoshi-ota * fix(safety_check): fix polygon edge Signed-off-by: satoshi-ota * fix(safety_check): fix param name Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 1 + .../parameter_helper.hpp | 2 + .../path_safety_checker_parameters.hpp | 3 + .../path_safety_checker/safety_check.cpp | 104 +++++++++++++++++- 4 files changed, 106 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index d9ae9ed623cb3..ab4eda81f93c6 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -187,6 +187,7 @@ time_horizon_for_rear_object: 10.0 # [s] delay_until_departure: 0.0 # [s] # rss parameters + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] rear_vehicle_reaction_time: 2.0 # [s] 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 03902e4328eb2..fcaad191b1a33 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 @@ -223,6 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // safety check rss params { const std::string ns = "avoidance.safety_check."; + p.rss_params.extended_polygon_policy = + getOrDeclareParameter(*node, ns + "extended_polygon_policy"); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d10a2f38fb975..b3c8034a3545c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -126,6 +126,9 @@ struct TargetObjectsOnLane */ struct RSSparams { + std::string extended_polygon_policy{ + "rectangle"}; ///< Policy to create collision check polygon. + ///< possible values: ["rectangle", "along_path"] double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index f5d76f5cd4d35..4b0697dbe9605 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -184,6 +184,88 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +Polygon2d createExtendedPolygonAlongPath( + const PathWithLaneId & planned_path, const Pose & base_link_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length, + const double lat_margin, const double is_stopped_obj, CollisionCheckDebug & debug) +{ + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double lat_offset = width / 2.0 + lat_margin; + + { + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; + } + + const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose( + planned_path.points, base_link_pose.position, lon_length); + if (!lon_offset_pose.has_value()) { + return createExtendedPolygon( + base_link_pose, vehicle_info, lon_length, lat_margin, is_stopped_obj, debug); + } + + const size_t start_idx = + motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); + const size_t end_idx = + motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position); + + Polygon2d polygon; + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { + const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); + const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + for (size_t i = end_idx; i > start_idx; --i) { + const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); + const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const std::vector & predicted_path, const VehicleInfo & vehicle_info, const double forward_margin, const double backward_margin, const double lat_margin) @@ -552,10 +634,24 @@ std::vector getCollidedPolygons( const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; // TODO(watanabe) fix hard coding value const bool is_stopped_object = object_velocity < 0.3; - const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( - ego_pose, ego_vehicle_info, lon_offset, - lat_margin, is_stopped_object, debug) - : ego_polygon; + const auto extended_ego_polygon = [&]() { + if (!is_object_front) { + return ego_polygon; + } + + if (rss_parameters.extended_polygon_policy == "rectangle") { + return createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, debug); + } + + if (rss_parameters.extended_polygon_policy == "along_path") { + return createExtendedPolygonAlongPath( + planned_path, ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, + debug); + } + + throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'."); + }(); const auto & extended_obj_polygon = is_object_front ? obj_polygon From ca827ee014a12cd5af7e8901eb8b714e055de238 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 29 Feb 2024 00:19:56 +0900 Subject: [PATCH 37/48] feat(avoidance): wait next shift approval until the ego reaches shift length threshold (#6501) Signed-off-by: satoshi-ota Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../config/avoidance.param.yaml | 5 ++++ .../data_structs.hpp | 5 ++++ .../behavior_path_avoidance_module/helper.hpp | 27 +++++++++++++++++++ .../parameter_helper.hpp | 6 +++++ .../src/scene.cpp | 3 ++- 5 files changed, 45 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index ab4eda81f93c6..cd2a51851fe5f 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -208,6 +208,11 @@ max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter should be within range of 0.0 to 1.0. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.) + ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: min_prepare_time: 1.0 # [s] 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 a536fa1b568cb..6aace5a939f9b 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 @@ -276,6 +276,9 @@ struct AvoidanceParameters // use for judge if the ego is shifting or not. double lateral_avoid_check_threshold{0.0}; + // use for return shift approval. + double ratio_for_return_shift_approval{0.0}; + // For shift line generation process. The continuous shift length is quantized by this value. double quantize_filter_threshold{0.0}; @@ -542,6 +545,8 @@ struct AvoidancePlanningData bool valid{false}; + bool ready{false}; + bool success{false}; bool comfortable{false}; 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 cc00bfbb978f5..4f15261f42246 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 @@ -270,6 +270,33 @@ class AvoidanceHelper }); } + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const + { + if (std::abs(current_shift_length) < 1e-3) { + return true; + } + + if (new_shift_lines.empty()) { + return true; + } + + const auto front_shift_relative_length = new_shift_lines.front().getRelativeLength(); + + // same direction shift. + if (current_shift_length > 0.0 && front_shift_relative_length > 0.0) { + return true; + } + + // same direction shift. + if (current_shift_length < 0.0 && front_shift_relative_length < 0.0) { + return true; + } + + // keep waiting the other side shift approval until the ego reaches shift length threshold. + const auto ego_shift_ratio = (current_shift_length - getEgoShift()) / current_shift_length; + return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3; + } + bool isShifted() const { return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; 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 fcaad191b1a33..ce75fa03e96ad 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 @@ -258,6 +258,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); p.max_deviation_from_lane = getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + p.ratio_for_return_shift_approval = + getOrDeclareParameter(*node, ns + "ratio_for_return_shift_approval"); + if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) { + throw std::domain_error( + "ratio_for_return_shift_approval should be within range of 0.0 to 1.0"); + } } // avoidance maneuver (longitudinal) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 75529c35c2af5..cc15fe3b24bf7 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -133,7 +133,7 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid; + return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const @@ -513,6 +513,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); + data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()); } void AvoidanceModule::fillEgoStatus( From 36dc88ac41c924cb876b262e39a821534dfacfb3 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 38/48] 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 --- .../config/avoidance_by_lc.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_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml index 3e8907cdccdf6..ad5fe0b123f1d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.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 3f810710ef37b..d612b6f497033 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 2b2e0e36507c5..efd169c937abc 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 @@ -86,9 +86,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 cd2a51851fe5f..b3546f46b8b09 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 ce75fa03e96ad..13425b3118d82 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 a0a8e375118f0..90d4b4ae90c36 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -483,35 +483,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)) { @@ -547,7 +530,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; @@ -587,7 +570,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 = @@ -780,7 +763,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; @@ -846,10 +829,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 = @@ -1489,8 +1475,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) < @@ -1673,8 +1662,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); @@ -1718,7 +1710,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] @@ -1730,17 +1721,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; @@ -2048,8 +2050,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 74a463d38d45f90e8eb46dfed355fd4de7b847a0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Feb 2024 21:32:22 +0900 Subject: [PATCH 39/48] feat(motion_velocity_smoother): use common max_vel (#6388) Signed-off-by: Takayuki Murooka --- .../config/default_common.param.yaml | 2 ++ .../motion_velocity_smoother/config/default_common.param.yaml | 2 ++ .../src/motion_velocity_smoother_node.cpp | 4 ++-- .../obstacle_cruise_planner/config/default_common.param.yaml | 2 ++ planning/obstacle_stop_planner/config/common.param.yaml | 2 ++ planning/planning_test_utils/config/test_common.param.yaml | 2 ++ planning/static_centerline_optimizer/config/common.param.yaml | 2 ++ 7 files changed, 14 insertions(+), 2 deletions(-) diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/external_velocity_limit_selector/config/default_common.param.yaml index a23570a5fc791..be0667c158878 100644 --- a/planning/external_velocity_limit_selector/config/default_common.param.yaml +++ b/planning/external_velocity_limit_selector/config/default_common.param.yaml @@ -1,6 +1,8 @@ /**: ros__parameters: # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + normal: min_acc: -0.5 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] diff --git a/planning/motion_velocity_smoother/config/default_common.param.yaml b/planning/motion_velocity_smoother/config/default_common.param.yaml index a23570a5fc791..be0667c158878 100644 --- a/planning/motion_velocity_smoother/config/default_common.param.yaml +++ b/planning/motion_velocity_smoother/config/default_common.param.yaml @@ -1,6 +1,8 @@ /**: ros__parameters: # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + normal: min_acc: -0.5 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 90f7295c4cf6f..12abc2ea4f8f6 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -173,7 +173,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit); update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit); - update_param("max_velocity", p.max_velocity); + update_param("max_vel", p.max_velocity); update_param( "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit); update_param("replan_vel_deviation", p.replan_vel_deviation); @@ -277,7 +277,7 @@ void MotionVelocitySmootherNode::initCommonParam() p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); p.enable_steering_rate_limit = declare_parameter("enable_steering_rate_limit"); - p.max_velocity = declare_parameter("max_velocity"); // 72.0 kmph + p.max_velocity = declare_parameter("max_vel"); p.margin_to_insert_external_velocity_limit = declare_parameter("margin_to_insert_external_velocity_limit"); p.replan_vel_deviation = declare_parameter("replan_vel_deviation"); diff --git a/planning/obstacle_cruise_planner/config/default_common.param.yaml b/planning/obstacle_cruise_planner/config/default_common.param.yaml index f900d309123cf..617d148137231 100644 --- a/planning/obstacle_cruise_planner/config/default_common.param.yaml +++ b/planning/obstacle_cruise_planner/config/default_common.param.yaml @@ -1,6 +1,8 @@ /**: ros__parameters: # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] diff --git a/planning/obstacle_stop_planner/config/common.param.yaml b/planning/obstacle_stop_planner/config/common.param.yaml index a23570a5fc791..be0667c158878 100644 --- a/planning/obstacle_stop_planner/config/common.param.yaml +++ b/planning/obstacle_stop_planner/config/common.param.yaml @@ -1,6 +1,8 @@ /**: ros__parameters: # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + normal: min_acc: -0.5 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] diff --git a/planning/planning_test_utils/config/test_common.param.yaml b/planning/planning_test_utils/config/test_common.param.yaml index 6bb130e8052b0..6c547c8cd83dc 100644 --- a/planning/planning_test_utils/config/test_common.param.yaml +++ b/planning/planning_test_utils/config/test_common.param.yaml @@ -1,6 +1,8 @@ /**: ros__parameters: # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] diff --git a/planning/static_centerline_optimizer/config/common.param.yaml b/planning/static_centerline_optimizer/config/common.param.yaml index a23570a5fc791..be0667c158878 100644 --- a/planning/static_centerline_optimizer/config/common.param.yaml +++ b/planning/static_centerline_optimizer/config/common.param.yaml @@ -1,6 +1,8 @@ /**: ros__parameters: # constraints param for normal driving + max_vel: 11.1 # max velocity limit [m/s] + normal: min_acc: -0.5 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] From eb27925eb1718d3a050bcc5090c2dfb1bc875fd0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 15 Feb 2024 09:15:41 +0900 Subject: [PATCH 40/48] fix(external_velocity_limit_selector): fix parameter name (#6415) Signed-off-by: satoshi-ota --- .../src/external_velocity_limit_selector_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index ccca5e97d2b38..5e557167ac3e1 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -112,7 +112,7 @@ ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode( // Params { auto & p = node_param_; - p.max_velocity = this->declare_parameter("max_velocity"); + p.max_velocity = this->declare_parameter("max_vel"); p.normal_min_acc = this->declare_parameter("normal.min_acc"); p.normal_max_acc = this->declare_parameter("normal.max_acc"); p.normal_min_jerk = this->declare_parameter("normal.min_jerk"); From b8225f0c5d9dbc92db3dcccbcb7d3031f90e7e7d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 15 Mar 2024 11:29:07 +0900 Subject: [PATCH 41/48] fix(lane_change): consider max velocity during path planning (#6615) Signed-off-by: Muhammad Zulfaqar Azmi --- planning/behavior_path_lane_change_module/src/scene.cpp | 9 +++++---- .../src/behavior_path_planner_node.cpp | 1 + .../include/behavior_path_planner_common/parameters.hpp | 1 + 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f0972d66416c8..ce44ee27b6638 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1171,9 +1171,9 @@ bool NormalLaneChange::getLaneChangePaths( }; // get path on original lanes - const auto prepare_velocity = std::max( + const auto prepare_velocity = std::clamp( current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity); + minimum_lane_changing_velocity, getCommonParam().max_vel); // compute actual longitudinal acceleration const double longitudinal_acc_on_prepare = @@ -1237,8 +1237,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; - const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; + const auto terminal_lane_changing_velocity = std::min( + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, + getCommonParam().max_vel); utils::lane_change::setPrepareVelocity( prepare_segment, current_velocity, terminal_lane_changing_velocity); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 85abf774d159e..298111e1c27b2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -242,6 +242,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); + p.max_vel = declare_parameter("max_vel"); p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 5bdd0a2f3f88d..6decd8fd2a131 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -45,6 +45,7 @@ struct BehaviorPathPlannerParameters // common parameters double min_acc; double max_acc; + double max_vel; double minimum_pull_over_length; double minimum_pull_out_length; From aad79a40e28eae8b221c5081289baeaa35587f52 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Mar 2024 12:16:48 +0900 Subject: [PATCH 42/48] fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition (#6355) * fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition Signed-off-by: satoshi-ota * fix(avoidance): don't insert stop point when the path is invalid Signed-off-by: satoshi-ota * refactor(avoidance): update parameter namespace Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 5 +- .../data_structs.hpp | 3 - .../parameter_helper.hpp | 7 +-- .../behavior_path_avoidance_module/scene.hpp | 7 --- .../src/scene.cpp | 59 ++++++++++++------- 5 files changed, 42 insertions(+), 39 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index b3546f46b8b09..08d4cd3027153 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -9,8 +9,6 @@ # avoidance module common setting enable_bound_clipping: false - enable_yield_maneuver: true - enable_yield_maneuver_during_shifting: false enable_cancel_maneuver: true disable_path_update: false @@ -248,7 +246,8 @@ # For yield maneuver yield: - yield_velocity: 2.78 # [m/s] + enable: true # [-] + enable_during_shifting: false # [-] # For stop maneuver stop: 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..e60b52c5f76cc 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 @@ -215,9 +215,6 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; - // keep target velocity in yield maneuver - double yield_velocity{0.0}; - // maximum stop distance double stop_max_distance{0.0}; 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 13425b3118d82..a1400cf268ed9 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 @@ -41,9 +41,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); @@ -294,7 +291,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // yield { const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.enable_yield_maneuver_during_shifting = + getOrDeclareParameter(*node, ns + "enable_during_shifting"); } // stop diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 0bb480bfa26b1..bb727a11d6934 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -235,13 +235,6 @@ class AvoidanceModule : public SceneModuleInterface */ void insertPrepareVelocity(ShiftedPath & shifted_path) const; - /** - * @brief insert decel point in output path in order to yield. the ego decelerates within - * accel/jerk constraints. - * @param target path. - */ - void insertYieldVelocity(ShiftedPath & shifted_path) const; - /** * @brief calculate stop distance based on object's overhang. * @param stop distance. diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 49d21c15ff8a5..c526152c20211 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -687,7 +687,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif break; } case AvoidanceState::YIELD: { - insertYieldVelocity(path); insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } @@ -1503,6 +1502,11 @@ void AvoidanceModule::insertWaitPoint( { const auto & data = avoid_data_; + // If avoidance path is NOT valid, don't insert any stop points. + if (!data.valid) { + return; + } + if (!data.stop_target_object) { return; } @@ -1589,28 +1593,20 @@ void AvoidanceModule::insertStopPoint( getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const +void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & p = parameters_; const auto & data = avoid_data_; - if (data.target_objects.empty()) { + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.safe && !data.stop_target_object) { return; } - if (helper_->isShifted()) { + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.valid) { return; } - const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false); - utils::avoidance::insertDecelPoint( - getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_); -} - -void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const -{ - const auto & data = avoid_data_; - // do nothing if there is no avoidance target. if (data.target_objects.empty()) { return; @@ -1626,26 +1622,45 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const return; } - const auto object = data.target_objects.front(); + const auto itr = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return o.avoid_required; }); + + const auto object = [&]() -> std::optional { + if (!data.yield_required) { + return data.target_objects.front(); + } + + if (itr == data.target_objects.end()) { + return std::nullopt; + } + + return *itr; + }(); + + // do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary. + if (!object.has_value()) { + return; + } const auto enough_space = - object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK; if (!enough_space) { return; } // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_type = utils::getHighestProbLabel(object.value().object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + const auto avoid_margin = object_parameter.lateral_hard_margin * object.value().distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto shift_length = - helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); + const auto shift_length = helper_->getShiftLength( + object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin); // check slow down feasibility const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length); - const auto distance_to_object = object.longitudinal; + const auto distance_to_object = object.value().longitudinal; const auto remaining_distance = distance_to_object - min_avoid_distance; const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front()); if (remaining_distance < decel_distance) { @@ -1653,7 +1668,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const } // decide slow down lower limit. - const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; + const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( From 5b597a7b761cb930837f32ad00a4c5dd21d11037 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 43/48] 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 d612b6f497033..1ac643f375683 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 08d4cd3027153..a039801de2c09 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -154,7 +154,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 e60b52c5f76cc..c8dcdbdc9c94f 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 @@ -381,7 +381,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 @@ -417,6 +418,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 a1400cf268ed9..3f86e8e4ef52c 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 @@ -137,7 +137,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 c526152c20211..73aadc35c11b6 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -629,21 +629,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); @@ -1409,16 +1404,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 90d4b4ae90c36..c597f8aaf9dab 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -445,6 +445,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; @@ -492,6 +500,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; @@ -580,15 +592,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; @@ -619,6 +627,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 @@ -745,24 +757,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 { @@ -1722,6 +1727,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); From ca20c93415bb9a4a7830d4581dd459afd7df27e1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 19 Mar 2024 08:13:12 +0900 Subject: [PATCH 44/48] feat(avoidance): wait and see ambiguous stopped vehicle (#6631) * feat(avoidance): wait and see the ambiguous stopped vehicle Signed-off-by: satoshi-ota * fix(avoidance): wait and see objects around ego straight lane Signed-off-by: satoshi-ota * tmp Signed-off-by: satoshi-ota * refactor(avoidance): filtering logic for vehicle type objects Signed-off-by: satoshi-ota * fix(avoidance): wait with unsafe avoidance path Signed-off-by: satoshi-ota * fix(avoidance): use getRightLanelet Signed-off-by: satoshi-ota * refactor(avoidance): parameterize Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 11 +- .../config/avoidance.param.yaml | 6 +- .../data_structs.hpp | 7 +- .../behavior_path_avoidance_module/helper.hpp | 29 +++ .../parameter_helper.hpp | 13 +- .../src/debug.cpp | 2 + .../src/scene.cpp | 3 +- .../src/utils.cpp | 198 ++++++++++++------ 8 files changed, 186 insertions(+), 83 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 1ac643f375683..c53c272bcb6fa 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 @@ -133,10 +133,13 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { 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 = - getOrDeclareParameter(*node, ns + "time_threshold"); + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index a039801de2c09..6184daf34c7d6 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -156,8 +156,10 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: enable: false # [-] - time_threshold: 1.0 # [s] - distance_threshold: 1.0 # [m] + closest_distance_to_wait_and_see: 10.0 # [m] + condition: + time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.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 c8dcdbdc9c94f..572e2f3ee8231 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 @@ -100,7 +100,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_force_avoidance_for_stopped_vehicle{false}; + bool enable_avoidance_for_ambiguous_vehicle{false}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -184,8 +184,9 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; - double force_avoidance_distance_threshold{0.0}; + double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + double time_threshold_for_ambiguous_vehicle{0.0}; + double distance_threshold_for_ambiguous_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction 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 5ade0dd0062b0..6d2eb81b328f4 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 @@ -289,6 +289,35 @@ class AvoidanceHelper }); } + bool isReady(const ObjectDataArray & objects) const + { + if (objects.empty()) { + return true; + } + + const auto object = objects.front(); + + if (!object.is_ambiguous) { + return true; + } + + if (!object.avoid_margin.has_value()) { + return true; + } + + const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto desire_shift_length = + getShiftLength(object, is_object_on_right, object.avoid_margin.value()); + + const auto prepare_distance = getMinimumPrepareDistance(); + const auto constant_distance = getFrontConstantDistance(object); + const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); + + return object.longitudinal < + prepare_distance + constant_distance + avoidance_distance + + parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + } + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const { if (std::abs(current_shift_length) < 1e-3) { 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 3f86e8e4ef52c..abd0c017a6483 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 @@ -138,12 +138,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { 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 = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.force_avoidance_distance_threshold = - getOrDeclareParameter(*node, ns + "distance_threshold"); + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 3c80a62a4fe2c..7d029277fcbc4 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -579,6 +579,8 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); addObjects(data.other_objects, std::string("UnstableObject")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 73aadc35c11b6..60e645f25bbf0 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -513,7 +513,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); - data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()); + data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()) && + helper_->isReady(data.target_objects); } void AvoidanceModule::fillEgoStatus( diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index c597f8aaf9dab..1ab140914b9a8 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -492,7 +492,8 @@ bool isMergingToEgoLane(const ObjectData & object) } bool isParkedVehicle( - ObjectData & object, const std::shared_ptr & route_handler, + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { using lanelet::geometry::distance2d; @@ -589,57 +590,36 @@ bool isParkedVehicle( object.shiftable_ratio > parameters->object_check_shiftable_ratio; } - return is_left_side_parked_vehicle || is_right_side_parked_vehicle; -} - -bool isAmbiguousStoppedVehicle( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) -{ - const auto stop_time_longer_than_threshold = - object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (!stop_time_longer_than_threshold) { + if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { return false; } const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto is_moving_distance_longer_than_threshold = - tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->force_avoidance_distance_threshold; - - if (is_moving_distance_longer_than_threshold) { - return false; - } - - if (object.is_within_intersection) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); - return false; - } - - const auto rh = planner_data->route_handler; - - if ( - !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) && - !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane."); + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } - if (!object.is_on_ego_lane) { - return true; - } + return true; +} +bool isCloseToStopFactor( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle - bool not_parked_object = true; + bool is_close_to_stop_factor = false; // check traffic light const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); { - not_parked_object = + is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; } @@ -651,12 +631,89 @@ bool isAmbiguousStoppedVehicle( const auto stop_for_crosswalk = to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; - not_parked_object = not_parked_object || stop_for_crosswalk; + is_close_to_stop_factor = is_close_to_stop_factor || stop_for_crosswalk; } object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - return !not_parked_object; + return is_close_to_stop_factor; +} + +bool isNeverAvoidanceTarget( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->distance_threshold_for_ambiguous_vehicle; + if (is_moving_distance_longer_than_threshold) { + object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + return true; + } + + if (object.is_within_intersection) { + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + } + + if (object.is_on_ego_lane) { + if ( + planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() && + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); + return true; + } + } + + if (isCloseToStopFactor(object, data, planner_data, parameters)) { + if (object.is_on_ego_lane && !object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it."); + return true; + } + } + + return false; +} + +bool isObviousAvoidanceTarget( + ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data, + [[maybe_unused]] const std::shared_ptr & planner_data, + [[maybe_unused]] const std::shared_ptr & parameters) +{ + if (!object.is_within_intersection) { + if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle."); + return true; + } + + if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle."); + return true; + } + } + + if (!object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + } + + return false; } bool isSatisfiedWithCommonCondition( @@ -759,50 +816,56 @@ bool isSatisfiedWithVehicleCondition( { object.behavior = getObjectBehavior(object, parameters); 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 (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) { + if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { + return false; + } + + if (isObviousAvoidanceTarget(object, data, planner_data, parameters)) { return true; } - // check vehicle shift ratio - if (object.is_on_ego_lane) { - if (object.is_parked) { - return true; - } else { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - return false; - } + // from here, filtering for ambiguous vehicle. + + if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + object.reason = "AmbiguousStoppedVehicle"; + return false; } - // Object is on center line -> ignore. - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; - if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + const auto stop_time_longer_than_threshold = + object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; + if (!stop_time_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; return false; } if (object.is_within_intersection) { - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; + } + } else { + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; return true; } - if (object.behavior == ObjectData::Behavior::NONE) { - object.reason = "ParallelToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; } - } - if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::NONE) { + object.is_ambiguous = false; + return true; + } } - return true; + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; } bool isNoNeedAvoidanceBehavior( @@ -1729,7 +1792,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.is_parked = + filtering_utils::isParkedVehicle(o, data, planner_data->route_handler, parameters); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { From a34c0391179ee6f7d790801069644b65e274cc87 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 Apr 2024 12:13:57 +0900 Subject: [PATCH 45/48] feat(avoidance): limit acceleration during avoidance maneuver (#6698) Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 3 +- .../data_structs.hpp | 3 + .../behavior_path_avoidance_module/helper.hpp | 75 +++++++++++++++++++ .../parameter_helper.hpp | 2 + .../behavior_path_avoidance_module/scene.hpp | 6 ++ .../src/manager.cpp | 13 ++++ .../src/scene.cpp | 47 ++++++++++++ 7 files changed, 148 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 6184daf34c7d6..4cfc76b6ffae1 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -290,7 +290,8 @@ nominal_jerk: 0.5 # [m/sss] max_deceleration: -1.5 # [m/ss] max_jerk: 1.0 # [m/sss] - max_acceleration: 1.0 # [m/ss] + max_acceleration: 0.5 # [m/ss] + min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] shift_line_pipeline: trim: 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 572e2f3ee8231..48b16f1ff2b69 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 @@ -143,6 +143,9 @@ struct AvoidanceParameters // To prevent large acceleration while avoidance. double max_acceleration{0.0}; + // To prevent large acceleration while avoidance. + double min_velocity_to_limit_max_acceleration{0.0}; + // upper distance for envelope polygon expansion. double upper_distance_for_polygon_expansion{0.0}; 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 6d2eb81b328f4..4ff02df97bd89 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 @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace behavior_path_planner::helper::avoidance @@ -32,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::PlannerData; using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; +using tier4_autoware_utils::getPose; class AvoidanceHelper { @@ -61,6 +65,11 @@ class AvoidanceHelper geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } + geometry_msgs::msg::Point getEgoPosition() const + { + return data_->self_odometry->pose.pose.position; + } + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( @@ -262,6 +271,20 @@ class AvoidanceHelper return *itr; } + std::pair getDistanceToAccelEndPoint(const PathWithLaneId & path) + { + updateAccelEndPoint(path); + + if (!max_v_point_.has_value()) { + return std::make_pair(0.0, std::numeric_limits::max()); + } + + const auto start_idx = data_->findEgoIndex(path.points); + const auto distance = + calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position); + return std::make_pair(distance, max_v_point_.value().second); + } + double getFeasibleDecelDistance( const double target_velocity, const bool use_hard_constraints = true) const { @@ -367,6 +390,56 @@ class AvoidanceHelper return true; } + void updateAccelEndPoint(const PathWithLaneId & path) + { + const auto & p = parameters_; + const auto & a_now = data_->self_acceleration->accel.accel.linear.x; + if (a_now < 0.0) { + max_v_point_ = std::nullopt; + return; + } + + if (getEgoSpeed() < p->min_velocity_to_limit_max_acceleration) { + max_v_point_ = std::nullopt; + return; + } + + if (max_v_point_.has_value()) { + return; + } + + const auto v0 = getEgoSpeed() + p->buf_slow_down_speed; + + const auto t_neg_jerk = std::max(0.0, a_now - p->max_acceleration) / p->max_jerk; + const auto v_neg_jerk = v0 + a_now * t_neg_jerk + std::pow(t_neg_jerk, 2.0) / 2.0; + const auto x_neg_jerk = v0 * t_neg_jerk + a_now * std::pow(t_neg_jerk, 2.0) / 2.0 + + p->max_jerk * std::pow(t_neg_jerk, 3.0) / 6.0; + + const auto & v_max = data_->parameters.max_vel; + if (v_max < v_neg_jerk) { + max_v_point_ = std::nullopt; + return; + } + + const auto t_max_accel = (v_max - v_neg_jerk) / p->max_acceleration; + const auto x_max_accel = + v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0; + + const auto point = + calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel); + if (point.has_value()) { + max_v_point_ = std::make_pair(point.value(), v_max); + return; + } + + const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); + const auto t_end = + (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; + const auto v_end = v0 + p->max_acceleration * t_end; + + max_v_point_ = std::make_pair(getPose(path.points.back()), v_end); + } + void reset() { prev_reference_path_ = PathWithLaneId(); @@ -417,6 +490,8 @@ class AvoidanceHelper ShiftedPath prev_linear_shift_path_; lanelet::ConstLanelets prev_driving_lanes_; + + std::optional> max_v_point_; }; } // namespace behavior_path_planner::helper::avoidance 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 abd0c017a6483..a3ee7dc19e81f 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 @@ -330,6 +330,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + p.min_velocity_to_limit_max_acceleration = + getOrDeclareParameter(*node, ns + "min_velocity_to_limit_max_acceleration"); } // constraints (lateral) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index bb727a11d6934..ead834c68d01f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -235,6 +235,12 @@ class AvoidanceModule : public SceneModuleInterface */ void insertPrepareVelocity(ShiftedPath & shifted_path) const; + /** + * @brief insert max velocity in output path to limit acceleration. + * @param target path. + */ + void insertAvoidanceVelocity(ShiftedPath & shifted_path) const; + /** * @brief calculate stop distance based on object's overhang. * @param stop distance. diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 692e4260520f3..05d7fb40ba294 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -146,6 +146,19 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "nominal_deceleration", p->nominal_deceleration); + updateParam(parameters, ns + "nominal_jerk", p->nominal_jerk); + updateParam(parameters, ns + "max_deceleration", p->max_deceleration); + updateParam(parameters, ns + "max_jerk", p->max_jerk); + updateParam(parameters, ns + "max_acceleration", p->max_acceleration); + updateParam( + parameters, ns + "min_velocity_to_limit_max_acceleration", + p->min_velocity_to_limit_max_acceleration); + } + { const std::string ns = "avoidance.shift_line_pipeline."; updateParam( diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 60e645f25bbf0..1c04fc0ca1976 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -677,6 +677,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } insertPrepareVelocity(path); + insertAvoidanceVelocity(path); switch (data.state) { case AvoidanceState::NOT_AVOID: { @@ -1697,6 +1698,52 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_object); } +void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const +{ + const auto & data = avoid_data_; + + // do nothing if no shift line is approved. + if (path_shifter_.getShiftLines().empty()) { + return; + } + + // do nothing if there is no avoidance target. + if (data.target_objects.empty()) { + return; + } + + const auto [distance_to_accel_end_point, v_max] = + helper_->getDistanceToAccelEndPoint(shifted_path.path); + if (distance_to_accel_end_point < 1e-3) { + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto accel_distance = distance_to_accel_end_point - distance_from_ego; + if (accel_distance < 0.0) { + break; + } + + const double v_target_square = + v_max * v_max - 2.0 * parameters_->max_acceleration * accel_distance; + if (v_target_square < 1e-3) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = std::max(getEgoSpeed(), std::sqrt(v_target_square)); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target); + } + + slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + shifted_path.path.points, start_idx, distance_to_accel_end_point); +} + std::shared_ptr AvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now(); From ae4176c9a6c2635cf95035ba1d82e8077d74dd26 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 8 Apr 2024 19:50:15 +0900 Subject: [PATCH 46/48] fix(avoidance): add update param to use rqt reconfigure (#6759) * fix(avoidance): add update param to use rqt reconfigure Signed-off-by: satoshi-ota * Update planning/behavior_path_avoidance_module/src/manager.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_avoidance_module/src/manager.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --------- Signed-off-by: satoshi-ota Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../parameter_helper.hpp | 4 + .../src/manager.cpp | 98 +++++++++++++++++++ 2 files changed, 102 insertions(+) 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 a3ee7dc19e81f..fbfec58abe4da 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 @@ -313,6 +313,10 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_shorten_margin_immediately = getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + if (p.policy_approval != "per_shift_line" && p.policy_approval != "per_avoidance_maneuver") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); } diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 05d7fb40ba294..5ce63987621ed 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -91,6 +91,76 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorupper_distance_for_polygon_expansion); } + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p->object_parameters.count(object_type) == 0) { + return; + } + updateParam(parameters, ns, p->object_parameters.at(object_type).is_avoidance_target); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + updateParam( + parameters, ns + "object_check_goal_distance", p->object_check_goal_distance); + updateParam( + parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance); + updateParam( + parameters, ns + "threshold_distance_object_is_on_center", + p->threshold_distance_object_is_on_center); + updateParam( + parameters, ns + "object_check_shiftable_ratio", p->object_check_shiftable_ratio); + updateParam( + parameters, ns + "object_check_min_road_shoulder_width", + p->object_check_min_road_shoulder_width); + updateParam( + parameters, ns + "object_last_seen_threshold", p->object_last_seen_threshold); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + updateParam(parameters, ns + "static", p->use_static_detection_area); + updateParam( + parameters, ns + "min_forward_distance", p->object_check_min_forward_distance); + updateParam( + parameters, ns + "max_forward_distance", p->object_check_max_forward_distance); + updateParam(parameters, ns + "backward_distance", p->object_check_backward_distance); + } + + { + const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; + updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam( + parameters, ns + "closest_distance_to_wait_and_see", + p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + updateParam( + parameters, ns + "condition.time_threshold", p->time_threshold_for_ambiguous_vehicle); + updateParam( + parameters, ns + "condition.distance_threshold", p->distance_threshold_for_ambiguous_vehicle); + updateParam( + parameters, ns + "ignore_area.traffic_light.front_distance", + p->object_ignore_section_traffic_light_in_front_distance); + updateParam( + parameters, ns + "ignore_area.crosswalk.front_distance", + p->object_ignore_section_crosswalk_in_front_distance); + updateParam( + parameters, ns + "ignore_area.crosswalk.behind_distance", + p->object_ignore_section_crosswalk_behind_distance); + } + + { + const std::string ns = "avoidance.target_filtering.intersection."; + updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); + } + { const std::string ns = "avoidance.avoidance.lateral."; updateParam( @@ -107,6 +177,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "min_prepare_time", p->min_prepare_time); updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); + updateParam(parameters, ns + "min_prepare_distance", p->min_prepare_distance); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); } @@ -117,6 +188,33 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_buffer", p->stop_buffer); } + { + const std::string ns = "avoidance.policy."; + updateParam(parameters, ns + "make_approval_request", p->policy_approval); + updateParam(parameters, ns + "deceleration", p->policy_deceleration); + updateParam(parameters, ns + "lateral_margin", p->policy_lateral_margin); + updateParam( + parameters, ns + "use_shorten_margin_immediately", p->use_shorten_margin_immediately); + + if (p->policy_approval != "per_shift_line" && p->policy_approval != "per_avoidance_maneuver") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid policy. please select 'per_shift_line' or 'per_avoidance_maneuver'."); + } + + if (p->policy_deceleration != "best_effort" && p->policy_deceleration != "reliable") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid deceleration policy. Please select 'best_effort' or 'reliable'."); + } + + if (p->policy_lateral_margin != "best_effort" && p->policy_lateral_margin != "reliable") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid lateral margin policy. Please select 'best_effort' or 'reliable'."); + } + } + { const std::string ns = "avoidance.constrains.lateral."; From 7b9cb829f4dcadf88571fe3e7a9cb469c93ea7fe Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 Apr 2024 20:59:21 +0900 Subject: [PATCH 47/48] fix(avoidance): bug in the logic to judge whether it's a parked vehicle (#6768) fix(avoidance): adjacent lane check for ambiguous stopped vehicle Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/utils.hpp | 4 -- .../src/scene.cpp | 1 - .../src/utils.cpp | 45 +++---------------- 3 files changed, 5 insertions(+), 45 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 02d8d68cf7a56..14d3bd1800dad 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -85,10 +85,6 @@ lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset); - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 1c04fc0ca1976..0630cf869bc8e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -318,7 +318,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; - using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 1ab140914b9a8..ee1d2dd8726fd 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -668,9 +668,11 @@ bool isNeverAvoidanceTarget( } if (object.is_on_ego_lane) { - if ( - planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() && - planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) { + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); + if (right_lane.has_value() && left_lane.has_value()) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); return true; @@ -1312,43 +1314,6 @@ std::vector generateObstaclePolygonsForDrivableArea( return obstacles_for_drivable_area; } -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset) -{ - const auto & rh = planner_data->route_handler; - - lanelet::ConstLanelets target_lanelets{}; - for (const auto & lane : route_lanelets) { - auto l_offset = 0.0; - auto r_offset = 0.0; - - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (opt_left_lane) { - target_lanelets.push_back(opt_left_lane.value()); - } else { - l_offset = left_offset; - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (opt_right_lane) { - target_lanelets.push_back(opt_right_lane.value()); - } else { - r_offset = right_offset; - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (!right_opposite_lanes.empty()) { - target_lanelets.push_back(right_opposite_lanes.front()); - } - - const auto expand_lane = lanelet::utils::getExpandedLanelet(lane, l_offset, r_offset); - target_lanelets.push_back(expand_lane); - } - - return target_lanelets; -} - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data) { From 011d95fa4935f4f937919a6ec3af98b34242cd73 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 Apr 2024 22:14:00 +0900 Subject: [PATCH 48/48] fix(avoidance): fix margin param inconsistency (#6765) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/scene.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 0630cf869bc8e..9ed189ac29adc 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -632,7 +632,10 @@ void AvoidanceModule::fillDebugData( 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 + + const auto lateral_hard_margin = o_front.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto max_avoid_margin = lateral_hard_margin * o_front.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto avoidance_distance = helper_->getSharpAvoidanceDistance( @@ -1403,7 +1406,10 @@ 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.lateral_hard_margin * object.distance_factor + + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto avoid_margin = lateral_hard_margin * object.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); @@ -1647,7 +1653,10 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.value().object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.lateral_hard_margin * object.value().distance_factor + + const auto lateral_hard_margin = object.value().is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto avoid_margin = lateral_hard_margin * object.value().distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength( object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin);