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