diff --git a/codecov.yaml b/codecov.yaml index 0341bdfda9c56..a2b67da30c3bf 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -113,3 +113,8 @@ component_management: # - control/control_performance_analysis/** - control/obstacle_collision_checker/** # - control/predicted_path_checker/** + + - component_id: perception-tier-iv-maintained-packages + name: Perception TIER IV Maintained Packages + paths: + - perception/[^lidar_apollo_instance_segmentation]/** diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index b84f8a4bd3bd7..821e470054f04 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -64,6 +64,8 @@ class BicycleMotionModel : public MotionModel double lr_min = 1.0; // [m] minimum distance from the center to the rear wheel double max_vel = 27.8; // [m/s] maximum velocity, 100km/h double max_slip = 0.5236; // [rad] maximum slip angle, 30deg + double max_reverse_vel = + -1.389; // [m/s] maximum reverse velocity, -5km/h. The value is expected to be negative } motion_params_; public: diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 812b91fc8acf0..8165b126eda8e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -45,13 +45,14 @@ class CTRVMotionModel : public MotionModel // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s - double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s - double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s - double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 - double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 - double max_vel = 2.78; // [m/s] maximum velocity - double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s + double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 + double max_vel = 2.78; // [m/s] maximum velocity + double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double max_reverse_vel = -1.38; // [m/s] maximum reverse velocity, -5km/h } motion_params_; public: diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 3c088b8f64b39..399634b63bffe 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -226,15 +226,26 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (motion_params_.max_reverse_vel < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum slip angle if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index a838bf62e5bcb..6f63ecbdce06d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -203,15 +203,26 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (X_t(IDX::VEL) < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum yaw rate if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/planning/autoware_mission_planner/CMakeLists.txt b/planning/autoware_mission_planner/CMakeLists.txt index 74bc8ddbc0a32..939903c2999ff 100644 --- a/planning/autoware_mission_planner/CMakeLists.txt +++ b/planning/autoware_mission_planner/CMakeLists.txt @@ -38,7 +38,8 @@ pluginlib_export_plugin_description_file(autoware_mission_planner plugins/plugin if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_lanelet2_plugins_default_planner.cpp + test/test_lanelet2_plugins_default_planner.cpp + test/test_utility_functions.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}_lanelet2_plugins diff --git a/planning/autoware_mission_planner/test/test_utility_functions.cpp b/planning/autoware_mission_planner/test/test_utility_functions.cpp new file mode 100644 index 0000000000000..6cde09b7664a2 --- /dev/null +++ b/planning/autoware_mission_planner/test/test_utility_functions.cpp @@ -0,0 +1,423 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include <../src/lanelet2_plugins/utility_functions.hpp> +#include +#include +#include + +#include +#include +#include +#include +#include + +using autoware::mission_planner::lanelet2::convert_linear_ring_to_polygon; +using autoware::mission_planner::lanelet2::convertBasicPoint3dToPose; +using autoware::mission_planner::lanelet2::convertCenterlineToPoints; +using autoware::mission_planner::lanelet2::exists; +using autoware::mission_planner::lanelet2::get_closest_centerline_pose; +using autoware::mission_planner::lanelet2::insert_marker_array; +using autoware::mission_planner::lanelet2::is_in_lane; +using autoware::mission_planner::lanelet2::is_in_parking_lot; +using autoware::mission_planner::lanelet2::is_in_parking_space; +using autoware::mission_planner::lanelet2::project_goal_to_map; + +using autoware::vehicle_info_utils::VehicleInfo; +using geometry_msgs::msg::Pose; + +TEST(TestUtilityFunctions, convertLinearRingToPolygon) +{ + // clockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + for (std::size_t i = 0; i < footprint.size(); ++i) { + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[i])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[i])); + } + + EXPECT_EQ(polygon.outer().front(), polygon.outer().back()); + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } + + // counterclockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + + // polygon is always clockwise + for (std::size_t i = 0; i < footprint.size(); ++i) { + const std::size_t j = footprint.size() - i - 1; + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[j])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[j])); + } + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } +} + +TEST(TestUtilityFunctions, convertCenterlineToPoints) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + const std::vector points = convertCenterlineToPoints(lanelet); + + ASSERT_EQ(points.size(), centerline.size()); + for (std::size_t i = 0; i < centerline.size(); ++i) { + EXPECT_DOUBLE_EQ(points[i].x, centerline[i].x()); + EXPECT_DOUBLE_EQ(points[i].y, centerline[i].y()); + EXPECT_DOUBLE_EQ(points[i].z, centerline[i].z()); + } +} + +TEST(TestUtilityFunctions, insertMarkerArray) +{ + visualization_msgs::msg::MarkerArray a1; + visualization_msgs::msg::MarkerArray a2; + a1.markers.resize(1); + a2.markers.resize(2); + a1.markers[0].id = 0; + a2.markers[0].id = 1; + a2.markers[1].id = 2; + + insert_marker_array(&a1, a2); + + ASSERT_EQ(a1.markers.size(), 3); + EXPECT_EQ(a1.markers[0].id, 0); + EXPECT_EQ(a1.markers[1].id, 1); + EXPECT_EQ(a1.markers[2].id, 2); +} + +TEST(TestUtilityFunctions, convertBasicPoint3dToPose) +{ + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = 0.0; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.w, 1.0); + } + + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = M_PI_2; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.7071067811865476); + EXPECT_DOUBLE_EQ(pose.orientation.w, 0.7071067811865476); + } +} + +TEST(TestUtilityFunctions, is_in_lane) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_lot) +{ + lanelet::Polygon3d parking_lot; + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_space) +{ + lanelet::LineString3d parking_space; + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + parking_space.setAttribute("width", 2.0); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } +} + +TEST(TestUtilityFunctions, project_goal_to_map) +{ + const auto create_lane = [&](const double height) -> lanelet::Lanelet { + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1, height}); + const lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + return lanelet; + }; + + const auto check_height = [&](const double height) -> void { + const auto lanelet = create_lane(height); + lanelet::Point3d goal_point{lanelet::InvalId, 0, 0, height}; + EXPECT_DOUBLE_EQ(project_goal_to_map(lanelet, goal_point), height); + }; + + check_height(0.0); + check_height(1.0); + check_height(-1.0); +} + +TEST(TestUtilityFunctions, TestUtilityFunctions) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + VehicleInfo vehicle_info; + vehicle_info.left_overhang_m = 0.5; + vehicle_info.right_overhang_m = 0.5; + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index 67aa41a5af7e5..1092047e65030 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -35,7 +35,7 @@ class PathDecisionState }; DecisionKind state{DecisionKind::NOT_DECIDED}; - rclcpp::Time stamp{}; + std::optional deciding_start_time{std::nullopt}; bool is_stable_safe{false}; std::optional safe_start_time{std::nullopt}; }; @@ -43,7 +43,7 @@ class PathDecisionState class PathDecisionStateController { public: - PathDecisionStateController() = default; + explicit PathDecisionStateController(rclcpp::Logger logger) : logger_(logger) {} /** * @brief update current state and save old current state to prev state @@ -62,11 +62,10 @@ class PathDecisionStateController PathDecisionState get_current_state() const { return current_state_; } - PathDecisionState get_prev_state() const { return prev_state_; } - private: + rclcpp::Logger logger_; + PathDecisionState current_state_{}; - PathDecisionState prev_state_{}; /** * @brief update current state and save old current state to prev state diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 12d564237db3c..70c9fd528b6a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -321,7 +321,7 @@ class GoalPlannerModule : public SceneModuleInterface // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() - PathDecisionStateController path_decision_controller_{}; + PathDecisionStateController path_decision_controller_{getLogger()}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 55cbd4c0e29d5..710700c1f6dc1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2643,7 +2643,6 @@ void PathDecisionStateController::transit_state( found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, pull_over_path, ego_polygons_expanded); - prev_state_ = current_state_; current_state_ = next_state; } @@ -2658,10 +2657,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const { - auto next_state = prev_state_; - - // update timestamp - next_state.stamp = now; + auto next_state = current_state_; // update safety if (!parameters.safety_check_params.enable_safety_check) { @@ -2684,7 +2680,6 @@ PathDecisionState PathDecisionStateController::get_next_state( // Once this function returns true, it will continue to return true thereafter if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; return next_state; } @@ -2699,12 +2694,16 @@ PathDecisionState PathDecisionStateController::get_next_state( // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } const auto & current_path = pull_over_path.getCurrentPath(); - if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { + if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { const double hysteresis_factor = 0.9; // check goal pose collision @@ -2712,7 +2711,9 @@ PathDecisionState PathDecisionStateController::get_next_state( modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data, static_target_objects)) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } @@ -2727,24 +2728,36 @@ PathDecisionState PathDecisionStateController::get_next_state( /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, ego_polygons_expanded, true)) { + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } if (enable_safety_check && !next_state.is_stable_safe) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); + const double elapsed_time_from_deciding = + (now - current_state_.deciding_start_time.value()).seconds(); if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); next_state.state = PathDecisionState::DecisionKind::DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); return next_state; } @@ -2766,10 +2779,15 @@ PathDecisionState PathDecisionStateController::get_next_state( // if object recognition for path collision check is enabled, transition to DECIDING to check // collision for a certain period of time. Otherwise, transition to DECIDED directly. if (parameters.use_object_recognition) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + next_state.state = PathDecisionState::DecisionKind::DECIDING; + next_state.deciding_start_time = now; return next_state; } - return {PathDecisionState::DecisionKind::DECIDED, now}; + return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index f6535e7adb8f8..c2b05929d597d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -73,7 +73,7 @@ std::optional PullOverPath::create( double parking_path_max_curvature{}; std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(full_path); + calculateCurvaturesAndMax(parking_path); return PullOverPath( type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index f76d776150875..68ff887e65522 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -142,9 +142,8 @@ class NormalLaneChange : public LaneChangeBase FilteredByLanesObjects filterObjectsByLanelets( const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const override; + bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const override; PathWithLaneId getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, @@ -155,17 +154,21 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + std::vector get_prepare_metrics() const; + std::vector get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, + const double shift_length, const double dist_to_reg_element) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; LaneChangePath get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const; + const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const; + const bool is_stuck) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 72a40caca1d6a..580c5709cb5c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -238,9 +238,8 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const = 0; + virtual bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index fc5e78e44b77f..4cdd3d1cbad3e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -338,6 +338,11 @@ struct TransientData double max_prepare_length{ std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + double target_lane_length{std::numeric_limits::min()}; + + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes + lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + bool is_ego_near_current_terminal_start{false}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 363fa970f54c4..8103434e34a61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -79,9 +79,7 @@ std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -104,12 +102,9 @@ ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, - const double next_lane_change_buffer); +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval); std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, @@ -145,8 +140,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug); + const std::vector & objects, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a91aaba86bb1a..2bfde9d400aeb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -129,6 +129,15 @@ void NormalLaneChange::update_transient_data() transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + transient_data.target_lane_length = + lanelet::utils::getLaneletLength2d(common_data_ptr_->lanes_ptr->target); + + transient_data.current_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + + transient_data.target_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; @@ -166,8 +175,7 @@ void NormalLaneChange::updateLaneChangeStatus() status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); - status_.start_distance = arclength_start.length; + status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -233,10 +241,7 @@ bool NormalLaneChange::is_near_regulatory_element() const { if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - if (dist_to_terminal_start <= max_prepare_length) return false; + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; @@ -244,8 +249,9 @@ bool NormalLaneChange::is_near_regulatory_element() const RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( - common_data_ptr_, only_tl, only_tl); + return common_data_ptr_->transient_data.max_prepare_length > + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -593,12 +599,13 @@ std::optional NormalLaneChange::extendPath() } auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + const auto & transient_data = common_data_ptr_->transient_data; const auto forward_path_length = getCommonParam().forward_path_length; - if ((target_lane_length - dist_in_target.length) > forward_path_length) { + if ( + (transient_data.target_lane_length - transient_data.target_lanes_ego_arc.length) > + forward_path_length) { return std::nullopt; } @@ -646,6 +653,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); } + void NormalLaneChange::resetParameters() { is_abort_path_approved_ = false; @@ -662,11 +670,10 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & pose = getEgoPose(); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; - const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + const auto current_shift_length = common_data_ptr_->transient_data.current_lanes_ego_arc.distance; constexpr bool is_driving_forward = true; // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. @@ -789,7 +796,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -1037,21 +1044,41 @@ std::vector NormalLaneChange::calcPrepareDuration( return prepare_durations; } -PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const +bool NormalLaneChange::get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const { - if (current_lanes.empty()) { - return PathWithLaneId(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + return false; } - auto prepare_segment = prev_module_output_.path; + prepare_segment = prev_module_output_.path; const size_t current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - return prepare_segment; + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; } lane_change::TargetObjects NormalLaneChange::getTargetObjects( @@ -1370,73 +1397,90 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const +std::vector NormalLaneChange::get_prepare_metrics() const { - lane_change_debug_.collision_check_objects.clear(); - - const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); - - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return false; - } - - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - - // get velocity + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto current_velocity = getEgoVelocity(); // get sampling acceleration values const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; + return calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); +} + +std::vector NormalLaneChange::get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, + const double shift_length, const double dist_to_reg_element) const +{ + const auto & route_handler = getRouteHandler(); + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prep_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + transient_data.is_ego_near_current_terminal_start + ? transient_data.dist_to_terminal_end - prep_metric.length + : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; + auto target_lane_buffer = lane_change_parameters_->lane_change_finish_judge_buffer + + transient_data.next_dist_buffer.min; + if (std::abs(route_handler->getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; + } + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; + return calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); +} - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const +{ + lane_change_debug_.collision_check_objects.clear(); + + if (!common_data_ptr_->is_lanes_available()) { + RCLCPP_WARN(logger_, "lanes are not available. Not expected."); + return false; + } + if (common_data_ptr_->lanes_polygon_ptr->target_neighbor.empty()) { + RCLCPP_WARN(logger_, "target_lane_neighbors_polygon_2d is empty. Not expected."); + return false; + } + + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + const auto is_stuck = isVehicleStuck(current_lanes); + const auto current_velocity = getEgoVelocity(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * - prepare_durations.size()); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - - const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, dist_to_terminal_start); auto check_length_diff = [&](const double prep_length, const double lc_length, const bool check_lc) { @@ -1452,10 +1496,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) }; for (const auto & prep_metric : prepare_phase_metrics) { - const auto debug_print = [&](const auto & s) { + const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, - prep_metric.actual_lon_accel, prep_metric.length); + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); }; if (!check_length_diff(prep_metric.length, 0.0, false)) { @@ -1463,59 +1507,26 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) continue; } - auto prepare_segment = - getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); - - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); - continue; - } - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); - continue; - } - - // lane changing start is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start is behind target lanelet!"); + PathWithLaneId prepare_segment; + try { + if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + debug_print(e.what()); break; } debug_print("Prepare path satisfy constraints"); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - const auto max_lane_changing_length = std::invoke([&]() { - double max_length = - dist_to_terminal_start > max_prepare_length - ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length - : dist_to_terminal_end - prep_metric.length; - auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; - if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { - target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; - } - max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); - return max_length; - }); - - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( - common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, - prep_metric.sampled_lon_accel, max_lane_changing_length); + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); @@ -1535,7 +1546,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); + shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1543,20 +1554,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); - bool is_safe = false; try { - is_safe = check_candidate_path_safety( - candidate_path, target_objects, current_min_dist_buffer, is_stuck); + if (check_candidate_path_safety(candidate_path, target_objects, is_stuck)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; + } } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; } - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } - debug_print_lat("Reject: sampled path is not safe."); } } @@ -1568,19 +1575,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath NormalLaneChange::get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const + const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & forward_path_length = planner_data_->parameters.forward_path_length; const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, - forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); if (target_lane_reference_path.points.empty()) { throw std::logic_error("target_lane_reference_path is empty!"); @@ -1614,12 +1618,12 @@ LaneChangePath NormalLaneChange::get_candidate_path( bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const + const bool is_stuck) const { if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { + lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } @@ -1669,9 +1673,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto forward_path_length = common_parameters.forward_path_length; const auto minimum_lane_changing_velocity = lane_change_parameters_->minimum_lane_changing_velocity; @@ -1680,10 +1682,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = @@ -1722,6 +1721,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, minimum_lane_changing_velocity, next_min_dist_buffer); @@ -1752,10 +1752,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_min_dist_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1796,11 +1794,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, - debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -2214,8 +2209,7 @@ bool NormalLaneChange::isVehicleStuck( } // Check if any stationary object exist in obstacle_check_distance - using lanelet::utils::getArcCoordinates; - const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + const auto base_distance = common_data_ptr_->transient_data.current_lanes_ego_arc.length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point @@ -2225,7 +2219,8 @@ bool NormalLaneChange::isVehicleStuck( continue; } - const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + const auto ego_to_obj_dist = + lanelet::utils::getArcCoordinates(current_lanes, p).length - base_distance; if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); return true; // Stationary object is in front of ego. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 8794d79a4d10f..98da9112bbc09 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -322,12 +322,17 @@ std::optional construct_candidate_path( return std::optional{candidate_path}; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) { + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -337,10 +342,10 @@ PathWithLaneId getReferencePathFromTargetLane( if (is_goal_in_route) { const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lane_change_buffer; + next_lc_buffer; return std::min(dist_from_lc_start, s_goal); } - return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); }); constexpr double epsilon = 1e-4; @@ -548,10 +553,13 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto rough_shift_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; @@ -817,8 +825,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug) + const std::vector & objects, CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -868,7 +875,7 @@ bool passed_parked_objects( }); // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { return true; }