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)); }