diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index 563350dbe53cc..d870bcf9974a1 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include namespace autoware::test_utils @@ -54,14 +56,32 @@ lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; + if (!errors.empty()) { + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; } - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + for (lanelet::Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } } - return nullptr; + + // realign lanelet borders using updated points + for (lanelet::Lanelet lanelet : map->laneletLayer) { + auto left = lanelet.leftBound(); + auto right = lanelet.rightBound(); + std::tie(left, right) = lanelet::geometry::align(left, right); + lanelet.setLeftBound(left); + lanelet.setRightBound(right); + } + + return map; } LaneletMapBin convertToMapBinMsg( diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp index 35a2fe3ef45da..9a4081c51be3b 100644 --- a/planning/autoware_route_handler/test/test_route_handler.cpp +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -49,10 +49,11 @@ TEST_F(TestRouteHandler, getGoalLaneId) TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) { - set_route_handler("/test_map/overlap_map.osm"); + set_route_handler("overlap_map.osm"); ASSERT_FALSE(route_handler_->isHandlerReady()); - geometry_msgs::msg::Pose start_pose, goal_pose; + geometry_msgs::msg::Pose start_pose; + geometry_msgs::msg::Pose goal_pose; start_pose.position = autoware::universe_utils::createPoint(3728.870361, 73739.281250, 0); start_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); goal_pose.position = autoware::universe_utils::createPoint(3729.961182, 73727.328125, 0); @@ -79,11 +80,12 @@ TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) { - set_route_handler("/test_map/overlap_map.osm"); - set_test_route("/test_route/overlap_test_route.yaml"); + set_route_handler("overlap_map.osm"); + set_test_route("overlap_test_route.yaml"); ASSERT_TRUE(route_handler_->isHandlerReady()); - geometry_msgs::msg::Pose reference_pose, search_pose; + geometry_msgs::msg::Pose reference_pose; + geometry_msgs::msg::Pose search_pose; lanelet::ConstLanelet reference_lanelet; reference_pose.position = autoware::universe_utils::createPoint(3730.88, 73735.3, 0); @@ -102,71 +104,189 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) ASSERT_EQ(closest_lanelet.id(), 345); found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet( - search_pose, reference_lanelet, &closest_lanelet, 3.0, 1.046); + search_pose, reference_lanelet, &closest_lanelet, dist_threshold, yaw_threshold); ASSERT_TRUE(found_lanelet); ASSERT_EQ(closest_lanelet.id(), 277); } -// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) -// { -// lanelet::ConstLanelet closest_lane; - -// Pose search_pose; - -// search_pose.position = autoware::universe_utils::createPoint(-1.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained7 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); - -// ASSERT_TRUE(closest_lane_obtained7); -// ASSERT_EQ(closest_lane.id(), 4775); - -// search_pose.position = autoware::universe_utils::createPoint(-0.5, 1.75, 0); -// const auto closest_lane_obtained = -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); - -// ASSERT_TRUE(closest_lane_obtained); -// ASSERT_EQ(closest_lane.id(), 4775); - -// search_pose.position = autoware::universe_utils::createPoint(-.01, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained3 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, CheckLaneIsInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4785); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_TRUE(is_lane_in_goal_route_section); +} -// ASSERT_TRUE(closest_lane_obtained3); -// ASSERT_EQ(closest_lane.id(), 4775); +TEST_F(TestRouteHandler, CheckLaneIsNotInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4780); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_FALSE(is_lane_in_goal_route_section); +} -// search_pose.position = autoware::universe_utils::createPoint(0.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained1 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, checkGetLaneletSequence) +{ + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); -// ASSERT_TRUE(closest_lane_obtained1); -// ASSERT_EQ(closest_lane.id(), 4775); + lanelet::ConstLanelet closest_lanelet; + const auto found_closest_lanelet = route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + ASSERT_TRUE(found_closest_lanelet); + ASSERT_EQ(closest_lanelet.id(), 4765ul); + + const auto current_lanes = route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + + ASSERT_EQ(current_lanes.size(), 6ul); + ASSERT_EQ(current_lanes.at(0).id(), 4765ul); + ASSERT_EQ(current_lanes.at(1).id(), 4770ul); + ASSERT_EQ(current_lanes.at(2).id(), 4775ul); + ASSERT_EQ(current_lanes.at(3).id(), 4424ul); + ASSERT_EQ(current_lanes.at(4).id(), 4780ul); + ASSERT_EQ(current_lanes.at(5).id(), 4785ul); +} -// search_pose.position = autoware::universe_utils::createPoint(0.01, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained2 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneWhenLaneChangeToRight) +{ + const auto current_lanes = get_current_lanes(); + + // The input is within expectation. + // this lane is of preferred lane type + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // this alternative lane is a subset of preferred lane route section + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); + + // The input is within expectation. + // Although Direction::NONE, the function should still return result similar to + // Direction::RIGHT. + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // Although Direction::NONE is provided, the function should behave similarly to + // Direction::RIGHT. + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); +} -// ASSERT_TRUE(closest_lane_obtained2); -// ASSERT_EQ(closest_lane.id(), 4424); +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneUsingUnexpectedResults) +{ + const auto current_lanes = get_current_lanes(); -// search_pose.position = autoware::universe_utils::createPoint(0.5, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained4 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::LEFT); + ASSERT_EQ(result.size(), 0ul); + }); +} -// ASSERT_TRUE(closest_lane_obtained4); -// ASSERT_EQ(closest_lane.id(), 4424); +TEST_F(TestRouteHandler, testGetCenterLinePath) +{ + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + { + // The input is within expectation. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 50.0); + ASSERT_EQ(center_line_path.points.size(), 51); // 26 + 26 - 1(overlapped) + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 2); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4780); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(1), 4785); + } + { + // The input is broken. + // s_start is negative, and s_end is over the boundary. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, -1.0, 200.0); + ASSERT_EQ(center_line_path.points.size(), 76); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4785); + } +} +TEST_F(TestRouteHandler, DISABLED_testGetCenterLinePathWhenLanesIsNotConnected) +{ + // broken current lanes. 4424 and 4785 are not connected directly. + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + + // The input is broken. Test is disabled because it doesn't pass. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 75.0); + ASSERT_EQ(center_line_path.points.size(), 26); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4424); +} -// search_pose.position = autoware::universe_utils::createPoint(1.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained5 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) +{ + auto get_closest_lanelet_within_route = + [&](double x, double y, double z) -> std::optional { + const auto pose = autoware::test_utils::createPose(x, y, z, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + const auto closest_lane_obtained = + route_handler_->getClosestLaneletWithinRoute(pose, &closest_lanelet); + if (!closest_lane_obtained) { + return std::nullopt; + } + return closest_lanelet.id(); + }; + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.5, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.01, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.0, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.0, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.01, 1.75, 0).value(), 4424ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.5, 1.75, 0).value(), 4424ul); +} -// ASSERT_TRUE(closest_lane_obtained5); -// ASSERT_EQ(closest_lane.id(), 4424); -// } +TEST_F(TestRouteHandler, testGetLaneChangeTargetLanes) +{ + { + // The input is within expectation. + // There exist no lane changing lane since both 4770 and 4775 are preferred lane. + const auto current_lanes = route_handler_->getLaneletsFromIds({4770, 4775}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + ASSERT_FALSE(lane_change_lane.has_value()); + } + + { + // The input is within expectation. + // There exist lane changing lane since 4424 is subset of preferred lane 9598. + const auto current_lanes = route_handler_->getLaneletsFromIds({4775, 4424}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + EXPECT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } + + { + // The input is within expectation. + // There is a lane-changing lane. Within the maximum current lanes, there is an alternative lane + // to the preferred lane. Therefore, the lane-changing lane exists. + const auto current_lanes = get_current_lanes(); + const auto lane_change_lane = route_handler_->getLaneChangeTarget(current_lanes); + ASSERT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } +} } // namespace autoware::route_handler::test diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp index 86f7461fc7538..ec3368809df99 100644 --- a/planning/autoware_route_handler/test/test_route_handler.hpp +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -15,7 +15,6 @@ #ifndef TEST_ROUTE_HANDLER_HPP_ #define TEST_ROUTE_HANDLER_HPP_ -#include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_test_utils/mock_data_parser.hpp" #include "gtest/gtest.h" @@ -37,16 +36,16 @@ namespace autoware::route_handler::test { +using autoware::test_utils::get_absolute_path_to_lanelet_map; +using autoware::test_utils::get_absolute_path_to_route; using autoware_map_msgs::msg::LaneletMapBin; - class TestRouteHandler : public ::testing::Test { public: TestRouteHandler() { - autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); - set_route_handler("/test_map/2km_test.osm"); - set_test_route("/test_route/lane_change_test_route.yaml"); + set_route_handler("2km_test.osm"); + set_test_route(lane_change_right_test_route_filename); } TestRouteHandler(const TestRouteHandler &) = delete; @@ -55,26 +54,44 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler & operator=(TestRouteHandler &&) = delete; ~TestRouteHandler() override = default; - void set_route_handler(const std::string & relative_path) + void set_route_handler(const std::string & lanelet_map_filename) { route_handler_.reset(); - const auto lanelet2_path = autoware_test_utils_dir + relative_path; + const auto lanelet2_path = + get_absolute_path_to_lanelet_map(autoware_test_utils_dir, lanelet_map_filename); const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); route_handler_ = std::make_shared(map_bin_msg); } - void set_test_route(const std::string & route_path) + void set_test_route(const std::string & route_filename) { - const auto route_handler_dir = - ament_index_cpp::get_package_share_directory("autoware_route_handler"); - const auto rh_test_route = route_handler_dir + route_path; + const auto rh_test_route = + get_absolute_path_to_route(autoware_route_handler_dir, route_filename); route_handler_->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); } + lanelet::ConstLanelets get_current_lanes() + { + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + [[maybe_unused]] const auto found_closest_lanelet = + route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + return route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + } + std::shared_ptr route_handler_; - std::string autoware_test_utils_dir; - static constexpr double center_line_resolution = 5.0; + std::string autoware_test_utils_dir{"autoware_test_utils"}; + std::string autoware_route_handler_dir{"autoware_route_handler"}; + std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"}; + + static constexpr double center_line_resolution{5.0}; + static constexpr double dist_threshold{3.0}; + static constexpr double yaw_threshold{1.045}; + static constexpr double backward_path_length{5.0}; + static constexpr double forward_path_length{300.0}; }; } // namespace autoware::route_handler::test diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index a7a7b30e4eec0..2c3e716151df8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -135,7 +135,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto resample_interval = avoidance_parameters_->resample_interval_for_planning; data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); - data.current_lanelets = getCurrentLanes(); + data.current_lanelets = get_current_lanes(); fillAvoidanceTargetObjects(data, debug); @@ -275,7 +275,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const { - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "no empty lanes"); return std::numeric_limits::infinity(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index b0d9967e65ddd..509f38d52dd45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/interface.cpp src/manager.cpp src/scene.cpp + src/utils/calculation.cpp src/utils/markers.cpp src/utils/utils.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5790be377b7aa..e42e6e2d47738 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -331,8 +331,7 @@ title NormalLaneChange::filterObjects Method Execution Flow start group "Filter Objects by Class" { -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) +while (has not finished iterating through predicted object list) is (TRUE) if (current object type != param.object_types_to_check?) then (TRUE) #LightPink:Remove current object; else (FALSE) @@ -341,17 +340,15 @@ endif end while end group -if (object list is empty?) then (TRUE) +if (predicted object list is empty?) then (TRUE) :Return empty result; stop else (FALSE) endif group "Filter Oncoming Objects" #PowderBlue { -:Iterate through each object in target lane objects list; -while (has not finished iterating through object list?) is (TRUE) -:check object's yaw with reference to ego's yaw.; -if (yaw difference < 90 degree?) then (TRUE) +while (has not finished iterating through predicted object list?) is (TRUE) +if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE) :Keep current object; else (FALSE) if (object is stopping?) then (TRUE) @@ -363,31 +360,7 @@ endif endwhile end group -if (object list is empty?) then (TRUE) - :Return empty result; - stop -else (FALSE) -endif - -group "Filter Objects Ahead Terminal" #Beige { -:Calculate lateral distance from ego to current lanes center; - -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) - :Get current object's polygon; - :Initialize distance to terminal from object to max; - while (has not finished iterating through object polygon's vertices) is (TRUE) - :Calculate object's lateral distance to end of lane; - :Update minimum distance to terminal from object; - end while - if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE) - #LightPink:Remove current object; - else (FALSE) - endif -end while -end group - -if (object list is empty?) then (TRUE) +if (predicted object list is empty?) then (TRUE) :Return empty result; stop else (FALSE) @@ -395,21 +368,27 @@ endif group "Filter Objects By Lanelets" #LightGreen { -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) - :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.; - if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE) - :Add to target_lane_objects; - else (FALSE) - if (Object overlaps with backward target lanes?) then (TRUE) +while (has not finished iterating through predicted object list) is (TRUE) + :Calculate lateral distance diff; + if (Object in target lane polygon?) then (TRUE) + if (lateral distance diff > half of ego's width?) then (TRUE) + if (Object's physical position is before terminal point?) then (TRUE) :Add to target_lane_objects; else (FALSE) - if (Object in current lane polygon?) then (TRUE) - :Add to current_lane_objects; - else (FALSE) - :Add to other_lane_objects; - endif endif + else (FALSE) + endif + else (FALSE) + endif + + if (Object overlaps with backward target lanes?) then (TRUE) + :Add to target_lane_objects; + else (FALSE) + if (Object in current lane polygon?) then (TRUE) + :Add to current_lane_objects; + else (FALSE) + :Add to other_lane_objects; + endif endif end while @@ -426,13 +405,10 @@ endif group "Filter Target Lanes' objects" #LightCyan { -:Iterate through each object in target lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through target lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) :Keep current object; else (FALSE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -444,11 +420,8 @@ end group group "Filter Current Lanes' objects" #LightYellow { -:Iterate through each object in current lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through current lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -462,11 +435,8 @@ end group group "Filter Other Lanes' objects" #Lavender { -:Iterate through each object in other lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through other lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -478,7 +448,7 @@ while (has not finished iterating through object list) is (TRUE) endwhile end group -:Trasform the objects into extended predicted object and return them as lane_change_target_objects; +:Transform the objects into extended predicted object and return them as lane_change_target_objects; stop @enduml @@ -648,6 +618,61 @@ The last behavior will also occur if the ego vehicle has departed from the curre ![stop](./images/lane_change-cant_cancel_no_abort.png) +## Lane change completion checks + +To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. + +For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. + +If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. + +The process of determining lane change completion is shown in the following diagram. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Lane change completion judge + +start + +:Calculate distance from current ego pose to lane change end pose; + +if (Is ego velocity < 1.0?) then (YES) + :Set finish_judge_buffer to 0.0; +else (NO) + :Set finish_judge_buffer to lane_change_finish_judge_buffer; +endif + +if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) + if (Current ego pose is in target lanes' polygon?) then (YES) + :Lane change is completed; + stop + else (NO) +:Lane change is NOT completed; +stop + endif +else (NO) +endif + +if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) + :Lane change is NOT completed; + stop +else (NO) + :Calculate distance to the target lanes' centerline; + if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) + :Lane change is completed; + stop + else (NO) + :Lane change is NOT completed; + stop + endif +endif + +@enduml +``` + ## Parameters ### Essential lane change parameters @@ -661,7 +686,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | -| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | | `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | | `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | | `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | @@ -677,6 +701,16 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | | `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | +### Parameter to judge if lane change is completed + +The following parameters are used to judge lane change completion. + +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | ----- | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------- | +| `lane_change_finish_judge_buffer` | [m] | double | The longitudinal distance starting from the lane change end pose. | 2.0 | +| `finish_judge_lateral_threshold` | [m] | double | The lateral distance from targets lanes' centerline. Used in addition with `finish_judge_lateral_angle_deviation` | 0.1 | +| `finish_judge_lateral_angle_deviation` | [deg] | double | Ego angle deviation with reference to target lanes' centerline. Used in addition with `finish_judge_lateral_threshold` | 2.0 | + ### Lane change regulations | Name | Unit | Type | Description | Default value | @@ -739,6 +773,18 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +#### safety constraints specifically for stopped or parked vehicles + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.parked.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.parked.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.0 | +| `safety_check.parked.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.0 | +| `safety_check.parked.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 0.8 | +| `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | +| `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + ##### safety constraints to cancel lane change path | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index d2f695071649a..a7128a0dc546f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -6,7 +6,6 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] - lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] @@ -27,6 +26,11 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + skip_process: + longitudinal_distance_diff_threshold: + prepare: 0.5 + lane_changing: 0.5 + # safety check safety_check: allow_loose_check_for_cancel: true @@ -39,6 +43,14 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + parked: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.0 cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -109,7 +121,9 @@ overhang_tolerance: 0.0 # [m] unsafe_hysteresis_threshold: 10 # [/] - finish_judge_lateral_threshold: 0.2 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] + finish_judge_lateral_threshold: 0.1 # [m] + finish_judge_lateral_angle_deviation: 1.0 # [deg] # debug publish_debug_marker: false 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 69e9e90a236b9..3a11b0ed903dc 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 @@ -31,10 +31,10 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; -using data::lane_change::LanesPolygon; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using lane_change::LanesPolygon; using tier4_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; @@ -51,6 +51,8 @@ class NormalLaneChange : public LaneChangeBase NormalLaneChange & operator=(NormalLaneChange &&) = delete; ~NormalLaneChange() override = default; + void update_lanes(const bool is_approved) final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; @@ -80,7 +82,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approved_path_safety_status) override; - bool isRequiredStop(const bool is_object_coming_from_rear) override; + bool isRequiredStop(const bool is_trailing_object) override; bool isNearEndOfCurrentLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -90,6 +92,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbleToReturnCurrentLane() const override; + bool is_near_terminal() const final; + bool isEgoOnPreparePhase() const override; bool isAbleToStopSafely() const override; @@ -105,8 +109,6 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() override; protected: - lanelet::ConstLanelets getCurrentLanes() const override; - lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const override; @@ -120,25 +122,16 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - ExtendedPredictedObjects getTargetObjects( - const LaneChangeLanesFilteredObjects & predicted_objects, + lane_change::TargetObjects getTargetObjects( + const FilteredByLanesExtendedObjects & predicted_objects, const lanelet::ConstLanelets & current_lanes) const; - LaneChangeLanesFilteredObjects filterObjects( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + FilteredByLanesExtendedObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; - void filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const; - - void filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - std::vector & current_lane_objects, - std::vector & target_lane_objects, - std::vector & other_lane_objects) const; + FilteredByLanesObjects filterObjectsByLanelets( + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double backward_path_length, @@ -176,7 +169,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, - const ExtendedPredictedObjects & collision_check_objects, + const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const; @@ -190,6 +183,24 @@ class NormalLaneChange : public LaneChangeBase bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + /** + * @brief Checks if the given pose is a valid starting point for a lane change. + * + * This function determines whether the given pose (position) of the vehicle is within + * the area of either the target neighbor lane or the target lane itself. It uses geometric + * checks to see if the start point of the lane change is covered by the polygons representing + * these lanes. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which should include: + * - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes. + * @param pose The current pose of the vehicle + * + * @return `true` if the pose is within either the target neighbor lane or the target lane; + * `false` otherwise. + */ + bool is_valid_start_point( + const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const; + bool check_prepare_phase() const; double calcMaximumLaneChangeLength( @@ -203,6 +214,11 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } + const lanelet::ConstLanelets & get_target_lanes() const + { + return common_data_ptr_->lanes_ptr->target; + } + double stop_time_{0.0}; }; } // namespace autoware::behavior_path_planner 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 be1f328fd89df..65f57fa583817 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 @@ -39,10 +39,10 @@ namespace autoware::behavior_path_planner { using autoware::route_handler::Direction; using autoware::universe_utils::StopWatch; -using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using lane_change::PathSafetyStatus; using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase @@ -52,6 +52,7 @@ class LaneChangeBase std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) : lane_change_parameters_{std::move(parameters)}, + common_data_ptr_{std::make_shared()}, direction_{direction}, type_{type}, time_keeper_(std::make_shared()) @@ -64,6 +65,8 @@ class LaneChangeBase LaneChangeBase & operator=(LaneChangeBase &&) = delete; virtual ~LaneChangeBase() = default; + virtual void update_lanes(const bool is_approved) = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -90,13 +93,15 @@ class LaneChangeBase virtual bool isAbleToReturnCurrentLane() const = 0; + virtual bool is_near_terminal() const = 0; + virtual LaneChangePath getLaneChangePath() const = 0; virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; virtual bool isEgoOnPreparePhase() const = 0; - virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; + virtual bool isRequiredStop(const bool is_trailing_object) = 0; virtual PathSafetyStatus isApprovedPathSafe() const = 0; @@ -130,7 +135,7 @@ class LaneChangeBase const LaneChangeStatus & getLaneChangeStatus() const { return status_; } - const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; } + const lane_change::Debug & getDebugData() const { return lane_change_debug_; } const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } @@ -138,6 +143,11 @@ class LaneChangeBase const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } + const lanelet::ConstLanelets & get_current_lanes() const + { + return common_data_ptr_->lanes_ptr->current; + } + const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } @@ -155,7 +165,28 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - void setData(const std::shared_ptr & data) { planner_data_ = data; } + void setData(const std::shared_ptr & data) + { + planner_data_ = data; + if (!common_data_ptr_->bpp_param_ptr) { + common_data_ptr_->bpp_param_ptr = + std::make_shared(data->parameters); + } + + if (!common_data_ptr_->lanes_ptr) { + common_data_ptr_->lanes_ptr = std::make_shared(); + } + + if (!common_data_ptr_->lanes_polygon_ptr) { + common_data_ptr_->lanes_polygon_ptr = std::make_shared(); + } + + common_data_ptr_->self_odometry_ptr = data->self_odometry; + common_data_ptr_->route_handler_ptr = data->route_handler; + common_data_ptr_->lc_param_ptr = lane_change_parameters_; + common_data_ptr_->lc_type = type_; + common_data_ptr_->direction = direction_; + } void setTimeKeeper(const std::shared_ptr & time_keeper) { @@ -199,8 +230,6 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() = 0; protected: - virtual lanelet::ConstLanelets getCurrentLanes() const = 0; - virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; virtual PathWithLaneId getPrepareSegment( @@ -228,6 +257,7 @@ class LaneChangeBase std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; + lane_change::CommonDataPtr common_data_ptr_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; @@ -242,7 +272,7 @@ class LaneChangeBase LaneChangeModuleType type_{LaneChangeModuleType::NORMAL}; mutable StopWatch stop_watch_; - mutable data::lane_change::Debug lane_change_debug_; + mutable lane_change::Debug lane_change_debug_; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp new file mode 100644 index 0000000000000..3dc5e7ee62a57 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ + +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" + +namespace autoware::behavior_path_planner::utils::lane_change::calculation +{ +using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::LCParamPtr; + +/** + * @brief Calculates the distance from the ego vehicle to the terminal point. + * + * This function computes the distance from the current position of the ego vehicle + * to either the goal pose or the end of the current lane, depending on whether + * the vehicle's current lane is within the goal section. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which should include: + * - Non null `lanes_ptr` that points to the current lanes data. + * - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle. + * - Non null `route_handler_ptr` that contains the goal pose of the route. + * + * @return The distance to the terminal point (either the goal pose or the end of the current lane) + * in meters. + */ +double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); + +double calc_dist_from_pose_to_terminal_end( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, + const Pose & src_pose); + +/** + * @brief Calculates the minimum stopping distance to terminal start. + * + * This function computes the minimum stopping distance to terminal start based on the + * minimum lane changing velocity and the minimum longitudinal acceleration. It then + * compares this calculated distance with a pre-defined backward length buffer parameter + * and returns the larger of the two values to ensure safe lane changing. + * + * @param lc_param_ptr Shared pointer to an LCParam structure, which should include: + * - `minimum_lane_changing_velocity`: The minimum velocity required for lane changing. + * - `min_longitudinal_acc`: The minimum longitudinal acceleration used for deceleration. + * - `backward_length_buffer_for_end_of_lane`: A predefined backward buffer length parameter. + * + * @return The required backward buffer distance in meters. + */ +double calc_stopping_distance(const LCParamPtr & lc_param_ptr); +} // namespace autoware::behavior_path_planner::utils::lane_change::calculation + +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ 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 a65f3baff01d7..9ddeca87c12a8 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 @@ -17,16 +17,29 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include +#include +#include #include +#include + #include #include +#include #include #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using route_handler::Direction; +using route_handler::RouteHandler; +using utils::path_safety_checker::ExtendedPredictedObjects; + struct LateralAccelerationMap { std::vector base_vel; @@ -68,7 +81,7 @@ struct LateralAccelerationMap } }; -struct LaneChangeCancelParameters +struct CancelParameters { bool enable_on_prepare_phase{true}; bool enable_on_lane_changing_phase{false}; @@ -83,7 +96,7 @@ struct LaneChangeCancelParameters int unsafe_hysteresis_threshold{2}; }; -struct LaneChangeParameters +struct Parameters { // trajectory generation double backward_lane_length{200.0}; @@ -92,12 +105,11 @@ struct LaneChangeParameters int lateral_acc_sampling_num{10}; // lane change parameters - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; + double backward_length_buffer_for_end_of_lane{0.0}; + double backward_length_buffer_for_blocking_object{0.0}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; - double lane_change_finish_judge_buffer{3.0}; LateralAccelerationMap lane_change_lat_acc_map; // parked vehicle @@ -112,6 +124,9 @@ struct LaneChangeParameters double min_longitudinal_acc{-1.0}; double max_longitudinal_acc{1.0}; + double skip_process_lon_diff_th_prepare{0.5}; + double skip_process_lon_diff_th_lane_changing{1.0}; + // collision check bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; bool enable_collision_check_for_prepare_phase_in_intersection{true}; @@ -139,47 +154,57 @@ struct LaneChangeParameters bool allow_loose_check_for_cancel{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_parked{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort - LaneChangeCancelParameters cancel{}; + CancelParameters cancel{}; + // finish judge parameter + double lane_change_finish_judge_buffer{3.0}; double finish_judge_lateral_threshold{0.2}; + double finish_judge_lateral_angle_deviation{autoware::universe_utils::deg2rad(3.0)}; // debug marker bool publish_debug_marker{false}; }; -enum class LaneChangeStates { +enum class States { Normal = 0, Cancel, Abort, Stop, }; -struct LaneChangePhaseInfo +struct PhaseInfo { double prepare{0.0}; double lane_changing{0.0}; [[nodiscard]] double sum() const { return prepare + lane_changing; } - LaneChangePhaseInfo(const double _prepare, const double _lane_changing) + PhaseInfo(const double _prepare, const double _lane_changing) : prepare(_prepare), lane_changing(_lane_changing) { } }; -struct LaneChangeInfo +struct Lanes { - LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0}; - LaneChangePhaseInfo velocity{0.0, 0.0}; - LaneChangePhaseInfo duration{0.0, 0.0}; - LaneChangePhaseInfo length{0.0, 0.0}; + bool current_lane_in_goal_section{false}; + lanelet::ConstLanelets current; + lanelet::ConstLanelets target_neighbor; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; +struct Info +{ + PhaseInfo longitudinal_acceleration{0.0, 0.0}; + PhaseInfo velocity{0.0, 0.0}; + PhaseInfo duration{0.0, 0.0}; + PhaseInfo length{0.0, 0.0}; Pose lane_changing_start{}; Pose lane_changing_end{}; @@ -190,41 +215,109 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeTargetObjectIndices +template +struct LanesObjects { - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; + Object current_lane{}; + Object target_lane_leading{}; + Object target_lane_trailing{}; + Object other_lane{}; + + LanesObjects() = default; + LanesObjects( + Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane) + : current_lane(std::move(current_lane)), + target_lane_leading(std::move(target_lane_leading)), + target_lane_trailing(std::move(target_lane_trailing)), + other_lane(std::move(other_lane)) + { + } }; -struct LaneChangeLanesFilteredObjects +struct TargetObjects { - utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects target_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects other_lane{}; + ExtendedPredictedObjects leading; + ExtendedPredictedObjects trailing; + TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing) + : leading(std::move(leading)), trailing(std::move(trailing)) + { + } }; -enum class LaneChangeModuleType { +enum class ModuleType { NORMAL = 0, EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; -} // namespace autoware::behavior_path_planner -namespace autoware::behavior_path_planner::data::lane_change -{ struct PathSafetyStatus { bool is_safe{true}; - bool is_object_coming_from_rear{false}; + bool is_trailing_object{false}; + + PathSafetyStatus() = default; + PathSafetyStatus(const bool is_safe, const bool is_trailing_object) + : is_safe(is_safe), is_trailing_object(is_trailing_object) + { + } }; struct LanesPolygon { std::optional current; std::optional target; - std::vector target_backward; + std::optional expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; +using LanesPolygonPtr = std::shared_ptr; + +struct CommonData +{ + RouteHandlerPtr route_handler_ptr; + Odometry::ConstSharedPtr self_odometry_ptr; + BppParamPtr bpp_param_ptr; + LCParamPtr lc_param_ptr; + LanesPtr lanes_ptr; + LanesPolygonPtr lanes_polygon_ptr; + ModuleType lc_type; + Direction direction; + + [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + + [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + + [[nodiscard]] double get_ego_speed(bool use_norm = false) const + { + if (!use_norm) { + return get_ego_twist().linear.x; + } + + const auto x = get_ego_twist().linear.x; + const auto y = get_ego_twist().linear.y; + return std::hypot(x, y); + } }; -} // namespace autoware::behavior_path_planner::data::lane_change +using CommonDataPtr = std::shared_ptr; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using autoware_perception_msgs::msg::PredictedObject; +using utils::path_safety_checker::ExtendedPredictedObjects; +using LaneChangeModuleType = lane_change::ModuleType; +using LaneChangeParameters = lane_change::Parameters; +using LaneChangeStates = lane_change::States; +using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangeInfo = lane_change::Info; +using FilteredByLanesObjects = lane_change::LanesObjects>; +using FilteredByLanesExtendedObjects = lane_change::LanesObjects; +using LateralAccelerationMap = lane_change::LateralAccelerationMap; +} // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp index 3b8159dc3ad47..fc51a82a8a842 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -26,7 +26,7 @@ #include #include -namespace autoware::behavior_path_planner::data::lane_change +namespace autoware::behavior_path_planner::lane_change { using utils::path_safety_checker::CollisionCheckDebugMap; struct Debug @@ -35,7 +35,7 @@ struct Debug LaneChangePaths valid_paths; CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; - LaneChangeLanesFilteredObjects filtered_objects; + FilteredByLanesExtendedObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; geometry_msgs::msg::Pose ego_pose; lanelet::ConstLanelets current_lanes; @@ -55,7 +55,8 @@ struct Debug collision_check_objects.clear(); collision_check_objects_after_approval.clear(); filtered_objects.current_lane.clear(); - filtered_objects.target_lane.clear(); + filtered_objects.target_lane_leading.clear(); + filtered_objects.target_lane_trailing.clear(); filtered_objects.other_lane.clear(); execution_area.points.clear(); current_lanes.clear(); @@ -71,6 +72,6 @@ struct Debug is_abort = false; } }; -} // namespace autoware::behavior_path_planner::data::lane_change +} // namespace autoware::behavior_path_planner::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index c40db47adf280..d403e7e1436c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -29,8 +29,9 @@ namespace marker_utils::lane_change_markers { +using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; using autoware::behavior_path_planner::LaneChangePath; -using autoware::behavior_path_planner::data::lane_change::Debug; +using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -39,9 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); MarkerArray showFilteredObjects( - const ExtendedPredictedObjects & current_lane_objects, - const ExtendedPredictedObjects & target_lane_objects, - const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); + const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 77c603e3bd975..5623f03a22eb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -23,31 +23,31 @@ #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; -struct LaneChangePath +struct Path { PathWithLaneId path{}; ShiftedPath shifted_path{}; - PathWithLaneId prev_path{}; - LaneChangeInfo info{}; + Info info{}; }; -using LaneChangePaths = std::vector; -struct LaneChangeStatus +struct Status { - PathWithLaneId lane_follow_path{}; - LaneChangePath lane_change_path{}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector lane_change_lane_ids{}; + Path lane_change_path{}; bool is_safe{false}; bool is_valid_path{false}; double start_distance{0.0}; }; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using LaneChangePath = lane_change::Path; +using LaneChangePaths = std::vector; +using LaneChangeStatus = lane_change::Status; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ 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 4071c39568987..a270900b491c3 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 @@ -48,8 +48,9 @@ using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using data::lane_change::LanesPolygon; -using data::lane_change::PathSafetyStatus; +using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -105,17 +106,14 @@ lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const LaneChangeModuleType & type); -lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType & type); - bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); ShiftLine getLaneChangingShiftLine( @@ -134,7 +132,7 @@ PathWithLaneId getReferencePathFromTargetLane( const double next_lane_change_buffer); std::vector generateDrivableLanes( - const std::vector original_drivable_lanes, const RouteHandler & route_handler, + const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); std::vector generateDrivableLanes( @@ -176,10 +174,9 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, +bool passed_parked_objects( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( @@ -195,7 +192,8 @@ ExtendedPredictedObject transform( const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + const std::vector & collided_polygons, + const std::optional & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. @@ -295,9 +293,29 @@ double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes); +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); + +Pose to_pose( + const autoware::universe_utils::Point2d & point, + const geometry_msgs::msg::Quaternion & orientation); + +bool is_ahead_of_ego( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object); + +bool is_before_terminal( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object); + +double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); + +ExtendedPredictedObjects transform_to_extended_objects( + const CommonDataPtr & common_data_ptr, const std::vector & objects, + const bool check_prepare_phase); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index b55b41828081e..9354b117cb160 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -75,6 +75,7 @@ void LaneChangeInterface::updateData() { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); + module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -136,7 +137,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); @@ -246,6 +247,15 @@ bool LaneChangeInterface::canTransitFailureState() return true; } + if (module_type_->is_near_terminal()) { + log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + + if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { + log_debug_throttled("Module require stopping"); + } + return false; + } + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); @@ -275,7 +285,7 @@ bool LaneChangeInterface::canTransitFailureState() return false; } - if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { + if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { log_debug_throttled("Module require stopping"); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 22c93f7d4bfd5..42166c4dff0e0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -122,6 +122,23 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.lateral_distance_max_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); + p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_front_deceleration")); + p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_rear_deceleration")); + p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); + p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); + p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.lateral_distance_max_threshold")); + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( @@ -166,8 +183,6 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); p.minimum_lane_changing_velocity = std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); - p.lane_change_finish_judge_buffer = - getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -224,8 +239,15 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.cancel.unsafe_hysteresis_threshold = getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); + // finish judge parameters + p.lane_change_finish_judge_buffer = + getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); p.finish_judge_lateral_threshold = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); + const auto finish_judge_lateral_angle_deviation = + getOrDeclareParameter(*node, parameter("finish_judge_lateral_angle_deviation")); + p.finish_judge_lateral_angle_deviation = + autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation); // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); @@ -332,6 +354,13 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); } + { + const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold."; + updateParam(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare); + updateParam( + parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing); + } + { const std::string ns = "lane_change.safety_check.lane_expansion."; updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); 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 75efc65c2997c..0f96475429265 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 @@ -14,6 +14,7 @@ #include "autoware/behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -23,6 +24,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include #include @@ -41,9 +43,10 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::calcMinimumLaneChangeLength; -using utils::lane_change::createLanesPolygon; +using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; using utils::traffic_light::getDistanceToNextTrafficLight; +namespace calculation = utils::lane_change::calculation; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -54,6 +57,48 @@ NormalLaneChange::NormalLaneChange( stop_watch_.tic("stop_time"); } +void NormalLaneChange::update_lanes(const bool is_approved) +{ + if (is_approved) { + return; + } + + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); + + if (current_lanes.empty()) { + return; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + if (target_lanes.empty()) { + return; + } + + const auto is_same_lanes_with_prev_iteration = + utils::lane_change::is_same_lane_with_prev_iteration( + common_data_ptr_, current_lanes, target_lanes); + + if (is_same_lanes_with_prev_iteration) { + return; + } + + common_data_ptr_->lanes_ptr->current = current_lanes; + common_data_ptr_->lanes_ptr->target = target_lanes; + + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( + *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); + + common_data_ptr_->lanes_ptr->current_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(current_lanes.back()); + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( + *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + common_data_ptr_->lc_param_ptr->backward_lane_length); + + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); +} + void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -61,35 +106,23 @@ void NormalLaneChange::updateLaneChangeStatus() const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status - status_.current_lanes = status_.lane_change_path.info.current_lanes; - status_.target_lanes = status_.lane_change_path.info.target_lanes; status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.target_lanes); - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.target_lanes, getEgoPose()); + const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); status_.start_distance = arclength_start.length; status_.lane_change_path.path.header = getRouteHeader(); } std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const { - const auto current_lanes = getCurrentLanes(); + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - if (current_lanes.empty()) { - return {false, false}; - } - - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { - safe_path.info.current_lanes = current_lanes; + if (current_lanes.empty() || target_lanes.empty()) { return {false, false}; } - // find candidate paths LaneChangePaths valid_paths{}; const bool is_stuck = isVehicleStuck(current_lanes); bool found_safe_path = getLaneChangePaths( @@ -105,8 +138,6 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { - safe_path.info.current_lanes = current_lanes; - safe_path.info.target_lanes = target_lanes; return {false, false}; } @@ -122,21 +153,22 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { - status_.current_lanes = getCurrentLanes(); + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - if (status_.current_lanes.empty()) { + if (current_lanes.empty()) { return false; } - status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - return !status_.target_lanes.empty(); + return !target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const { return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - status_.current_lanes, status_.lane_change_path.path, planner_data_, + get_current_lanes(), status_.lane_change_path.path, planner_data_, status_.lane_change_path.info.length.sum()); } @@ -144,7 +176,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = getLaneChangeStatus().current_lanes; + const auto & current_lanes = get_current_lanes(); const auto is_valid = getLaneChangeStatus().is_valid_path; const auto & lane_change_path = getLaneChangeStatus().lane_change_path; const auto & lane_change_param = getLaneChangeParam(); @@ -237,7 +269,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const return output; } - const auto current_lanes = getCurrentLanes(); + const auto & current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); return prev_module_output_; @@ -269,13 +301,14 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); + insertStopPoint(get_current_lanes(), prev_module_output_.path); return prev_module_output_; } auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(status_.current_lanes, output.path); + insertStopPoint(get_current_lanes(), output.path); } else { output.path = getLaneChangePath().path; @@ -295,7 +328,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, output.path); + insertStopPoint(get_target_lanes(), output.path); } } @@ -316,7 +349,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *getRouteHandler(), status_.current_lanes, status_.target_lanes); + *getRouteHandler(), get_current_lanes(), get_target_lanes()); const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, @@ -362,20 +395,12 @@ void NormalLaneChange::insertStopPoint( } const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(status_.current_lanes, status_.target_lanes); + const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; - const auto is_valid_start_point = std::invoke([&]() -> bool { - auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( - status_.lane_change_path.info.lane_changing_start.position); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon( - *route_handler, status_.current_lanes, type_); - return boost::geometry::covered_by( - lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); - }); + const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; - if (!is_valid_start_point) { + if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); setStopPose(stop_point.point.pose); @@ -441,7 +466,8 @@ void NormalLaneChange::insertStopPoint( // [ego]> | <--- lane change margin ---> [obj]> // ---------------------------------------------------------- const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), + [&](const auto & o) { const auto v = std::abs(o.initial_twist.twist.linear.x); if (v > lane_change_parameters_->stop_velocity_threshold) { return false; @@ -450,7 +476,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(status_.target_lanes) + lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; @@ -477,7 +503,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( - status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { + get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); @@ -499,7 +525,7 @@ std::optional NormalLaneChange::extendPath() return std::nullopt; } - auto & target_lanes = status_.target_lanes; + 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()); @@ -570,7 +596,7 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & pose = getEgoPose(); - const auto & current_lanes = status_.current_lanes; + 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; @@ -585,11 +611,6 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const -{ - return utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); -} - lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const { @@ -667,26 +688,39 @@ bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; - const double dist_to_lane_change_end = utils::getSignedDistance( - current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); - double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; - - // If ego velocity is low, relax finish judge buffer - const double ego_velocity = getEgoVelocity(); - if (std::abs(ego_velocity) < 1.0) { - finish_judge_buffer = 0.0; - } + const auto & target_lanes = get_target_lanes(); + const double dist_to_lane_change_end = + utils::getSignedDistance(current_pose, lane_change_end, target_lanes); + + const auto finish_judge_buffer = std::invoke([&]() { + const double ego_velocity = getEgoVelocity(); + // If ego velocity is low, relax finish judge buffer + if (std::abs(ego_velocity) < 1.0) { + return 0.0; + } + return lane_change_parameters_->lane_change_finish_judge_buffer; + }); - const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; + const auto has_passed_end_pose = dist_to_lane_change_end + finish_judge_buffer < 0.0; lane_change_debug_.distance_to_lane_change_finished = dist_to_lane_change_end + finish_judge_buffer; - if (!reach_lane_change_end) { + if (has_passed_end_pose) { + const auto & lanes_polygon = common_data_ptr_->lanes_polygon_ptr->target; + return !boost::geometry::disjoint( + lanes_polygon.value(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position))); + } + + const auto yaw_deviation_to_centerline = + utils::lane_change::calc_angle_to_lanelet_segment(target_lanes, current_pose); + + if (yaw_deviation_to_centerline > lane_change_parameters_->finish_judge_lateral_angle_deviation) { return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -703,7 +737,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const } if (!utils::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters, + get_current_lanes(), getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -724,7 +758,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters, + get_current_lanes(), estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -735,6 +769,31 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return true; } +bool NormalLaneChange::is_near_terminal() const +{ + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + + if (current_lanes.empty()) { + return true; + } + + const auto & current_lanes_terminal = current_lanes.back(); + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto direction = common_data_ptr_->direction; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + const auto min_lane_changing_distance = calcMinimumLaneChangeLength( + route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); + + const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); + + const auto min_lc_dist_with_buffer = + backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; + const auto dist_from_ego_to_terminal_end = + calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + + return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; +} + bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; @@ -763,7 +822,7 @@ bool NormalLaneChange::isAbleToStopSafely() const if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters); + get_current_lanes(), estimated_pose, planner_data_->parameters); } } return true; @@ -941,34 +1000,32 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( return prepare_segment; } -ExtendedPredictedObjects NormalLaneChange::getTargetObjects( - const LaneChangeLanesFilteredObjects & filtered_objects, +lane_change::TargetObjects NormalLaneChange::getTargetObjects( + const FilteredByLanesExtendedObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const { - ExtendedPredictedObjects target_objects = filtered_objects.target_lane; + ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || is_stuck) { - target_objects.insert( - target_objects.end(), filtered_objects.current_lane.begin(), + leading_objects.insert( + leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); } const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; if (chk_obj_in_other_lanes) { - target_objects.insert( - target_objects.end(), filtered_objects.other_lane.begin(), filtered_objects.other_lane.end()); + leading_objects.insert( + leading_objects.end(), filtered_objects.other_lane.begin(), + filtered_objects.other_lane.end()); } - return target_objects; + return {leading_objects, filtered_objects.target_lane_trailing}; } -LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const { - const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->object_types_to_check); @@ -983,83 +1040,64 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( return {}; } - filterAheadTerminalObjects(objects, current_lanes); + const auto & current_lanes = get_current_lanes(); - if (objects.objects.empty()) { + if (current_lanes.empty()) { return {}; } - std::vector target_lane_objects; - std::vector current_lane_objects; - std::vector other_lane_objects; - - filterObjectsByLanelets( - objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, - other_lane_objects); + const auto & target_lanes = get_target_lanes(); - const auto is_within_vel_th = [](const auto & object) -> bool { - constexpr double min_vel_th = 1.0; - constexpr double max_vel_th = std::numeric_limits::max(); - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); - }; + if (target_lanes.empty()) { + return {}; + } const auto path = route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - if (path.points.empty()) { - return {}; - } - - const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - return max_dist_ego_to_obj >= 0.0; + const auto is_within_vel_th = [](const auto & object) -> bool { + constexpr double min_vel_th = 1.0; + constexpr double max_vel_th = std::numeric_limits::max(); + return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); }; utils::path_safety_checker::filterObjects( - target_lane_objects, [&](const PredictedObject & object) { - return (is_within_vel_th(object) || is_ahead_of_ego(object)); - }); - - utils::path_safety_checker::filterObjects( - other_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); - }); + filtered_by_lanes_objects.target_lane_trailing, + [&](const PredictedObject & object) { return is_within_vel_th(object); }); + + if (lane_change_parameters_->check_objects_on_other_lanes) { + utils::path_safety_checker::filterObjects( + filtered_by_lanes_objects.other_lane, [&](const PredictedObject & object) { + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) && ahead_of_ego; + }); + } utils::path_safety_checker::filterObjects( - current_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); + filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { + const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) && ahead_of_ego; }); - LaneChangeLanesFilteredObjects lane_change_target_objects; - const auto is_check_prepare_phase = check_prepare_phase(); - std::for_each(target_lane_objects.begin(), target_lane_objects.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - lane_change_target_objects.target_lane.push_back(extended_predicted_object); - }); - - std::for_each(current_lane_objects.begin(), current_lane_objects.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - lane_change_target_objects.current_lane.push_back(extended_predicted_object); - }); - - std::for_each(other_lane_objects.begin(), other_lane_objects.end(), [&](const auto & object) { - auto extended_predicted_object = utils::lane_change::transform( - object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); - lane_change_target_objects.other_lane.push_back(extended_predicted_object); - }); - + const auto target_lane_leading_extended_objects = + utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase); + const auto target_lane_trailing_extended_objects = + utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase); + const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase); + const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( + common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase); + + FilteredByLanesExtendedObjects lane_change_target_objects( + current_lane_extended_objects, target_lane_leading_extended_objects, + target_lane_trailing_extended_objects, other_lane_extended_objects); lane_change_debug_.filtered_objects = lane_change_target_objects; - return lane_change_target_objects; } @@ -1090,42 +1128,17 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -void NormalLaneChange::filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const +FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const { - const auto & current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), *lane_change_parameters_, direction_); - - const auto dist_ego_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - - // ignore object that are ahead of terminal lane change start - utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - // ignore object that are ahead of terminal lane change start - auto distance_to_terminal_from_object = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - Pose polygon_pose; - polygon_pose.position = obj_p; - polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; - const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); - distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist); - } - - return (minimum_lane_change_length > distance_to_terminal_from_object); - }); -} + std::vector target_lane_leading_objects; + std::vector target_lane_trailing_objects; + std::vector current_lane_objects; + std::vector other_lane_objects; -void NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, std::vector & current_lane_objects, - std::vector & target_lane_objects, - std::vector & other_lane_objects) const -{ const auto & current_pose = getEgoPose(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto check_optional_polygon = [](const auto & object, const auto & polygon) { @@ -1133,9 +1146,7 @@ void NormalLaneChange::filterObjectsByLanelets( }; // get backward lanes - const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = - utils::getPrecedingLanelets(*route_handler, target_lanes, current_pose, backward_length); + const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; { lane_change_debug_.current_lanes = current_lanes; @@ -1152,21 +1163,15 @@ void NormalLaneChange::filterObjectsByLanelets( }); } - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); - - const auto lanes_polygon = - createLanesPolygon(current_lanes, expanded_target_lanes, target_backward_lanes); + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - { - const auto reserve_size = objects.objects.size(); - current_lane_objects.reserve(reserve_size); - target_lane_objects.reserve(reserve_size); - other_lane_objects.reserve(reserve_size); - } + const auto reserve_size = objects.objects.size(); + current_lane_objects.reserve(reserve_size); + target_lane_leading_objects.reserve(reserve_size); + target_lane_trailing_objects.reserve(reserve_size); + other_lane_objects.reserve(reserve_size); for (const auto & object : objects.objects) { const auto is_lateral_far = std::invoke([&]() -> bool { @@ -1177,23 +1182,50 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) { - target_lane_objects.push_back(object); + const auto is_before_terminal = [&]() { + return utils::lane_change::is_before_terminal( + common_data_ptr_, current_lanes_ref_path, object); + }; + + if ( + check_optional_polygon(object, lanes_polygon.target) && is_lateral_far && + is_before_terminal()) { + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); + if (ahead_of_ego) { + target_lane_leading_objects.push_back(object); + } else { + target_lane_trailing_objects.push_back(object); + } continue; } + if ( + check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && + is_before_terminal()) { + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); + constexpr double stopped_obj_vel_th = 1.0; + if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { + if (ahead_of_ego) { + target_lane_leading_objects.push_back(object); + continue; + } + } + } + const auto is_overlap_target_backward = std::invoke([&]() -> bool { const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { return isPolygonOverlapLanelet(object, target_backward_polygon); }; return std::any_of( - lanes_polygon.target_backward.begin(), lanes_polygon.target_backward.end(), + lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), check_backward_polygon); }); // check if the object intersects with target backward lanes if (is_overlap_target_backward) { - target_lane_objects.push_back(object); + target_lane_trailing_objects.push_back(object); continue; } @@ -1205,6 +1237,10 @@ void NormalLaneChange::filterObjectsByLanelets( other_lane_objects.push_back(object); } + + return { + current_lane_objects, target_lane_leading_objects, target_lane_trailing_objects, + other_lane_objects}; } PathWithLaneId NormalLaneChange::getTargetSegment( @@ -1233,8 +1269,6 @@ PathWithLaneId NormalLaneChange::getTargetSegment( std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); }); - RCLCPP_DEBUG(logger_, "in %s start: %f, end: %f", __func__, s_start, s_end); - PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); for (auto & point : target_segment.points) { point.point.longitudinal_velocity_mps = @@ -1370,21 +1404,21 @@ bool NormalLaneChange::getLaneChangePaths( route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); const auto dist_to_end_of_current_lanes = - utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); + calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); 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 target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1398,12 +1432,6 @@ bool NormalLaneChange::getLaneChangePaths( for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - const auto debug_print = [&](const auto & s) { - RCLCPP_DEBUG_STREAM( - logger_, " - " << s << " : prep_time = " << prepare_duration - << ", lon_acc = " << sampled_longitudinal_acc); - }; - // get path on original lanes const auto prepare_velocity = std::clamp( current_velocity + sampled_longitudinal_acc * prepare_duration, @@ -1417,13 +1445,42 @@ bool NormalLaneChange::getLaneChangePaths( const auto prepare_length = utils::lane_change::calcPhaseLength( current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer; + if (prepare_length > ego_dist_to_terminal_start) { + RCLCPP_DEBUG( + logger_, + "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f, ego dist to " + "terminal start: %.5f", + prepare_length, ego_dist_to_terminal_start); + continue; + } + + if (!candidate_paths->empty()) { + const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; + if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); + continue; + } + } auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG( + logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s, + prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length); + }; + if (prepare_segment.points.empty()) { debug_print("prepare segment is empty...? Unexpected."); continue; } + 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 getEgoPose() 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( @@ -1454,13 +1511,11 @@ bool NormalLaneChange::getLaneChangePaths( } RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); - for (const auto & lateral_acc : sample_lat_acc) { - const auto debug_print = [&](const auto & s) { - RCLCPP_DEBUG_STREAM( - logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " - << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); - }; + debug_print("Prepare path satisfy constraints"); + 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); + for (const auto & lateral_acc : sample_lat_acc) { const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = @@ -1470,58 +1525,68 @@ bool NormalLaneChange::getLaneChangePaths( const auto lane_changing_length = utils::lane_change::calcPhaseLength( initial_lane_changing_velocity, getCommonParam().max_vel, 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); + + const auto debug_print_lat = [&](const auto & s) { + RCLCPP_DEBUG( + logger_, + " - %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s, + lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing, + lane_changing_length); + }; + if (!candidate_paths->empty()) { + const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; + const auto lc_length_diff = + candidate_paths->back().info.length.lane_changing - lane_changing_length; + + // We only check lc_length_diff if and only if the current prepare_length is equal to the + // previous prepare_length. + if ( + std::abs(prev_prep_diff) < eps && + std::abs(lc_length_diff) < + lane_change_parameters_->skip_process_lon_diff_th_lane_changing) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); + continue; + } + } if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } - if (is_goal_in_route) { - const double s_start = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; - const auto num = + // if multiple lane change is necessary, does the remaining distance is sufficient + const auto remaining_dist_in_target = std::invoke([&]() { + const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; + const auto num_to_preferred_lane_from_target_lane = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const double backward_buffer = - num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer; - if ( - s_start + lane_changing_length + finish_judge_buffer + backward_buffer + - next_lane_change_buffer > - s_goal) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } + const auto backward_len_buffer = + lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const auto backward_buffer_to_target_lane = + num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer; + return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane + + next_lane_change_buffer; + }); + + if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) { + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); + continue; } + 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); + const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - debug_print("Reject: target segment is empty!! something wrong..."); + debug_print_lat("Reject: target segment is empty!! something wrong..."); continue; } - const lanelet::BasicPoint2d lc_start_point( - prepare_segment.points.back().point.pose.position.x, - prepare_segment.points.back().point.pose.position.y); - - const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( - target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); - - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - LaneChangeInfo lane_change_info; lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; @@ -1529,20 +1594,11 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.velocity = LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = lateral_acc; lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - if (!is_valid_start_point) { - debug_print( - "Reject: lane changing points are not inside of the target preferred lanes or its " - "neighbors"); - continue; - } - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( lane_changing_length, initial_lane_changing_velocity); const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( @@ -1551,7 +1607,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - debug_print("Reject: target_lane_reference_path is empty!!"); + debug_print_lat("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1559,16 +1615,16 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, target_segment, target_lane_reference_path, shift_length); const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, prepare_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); if (!candidate_path) { - debug_print("Reject: failed to generate candidate path!!"); + debug_print_lat("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print("Reject: invalid candidate path!!"); + debug_print_lat("Reject: invalid candidate path!!"); continue; } @@ -1576,7 +1632,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including crosswalk!!"); + debug_print_lat("Reject: including crosswalk!!"); continue; } RCLCPP_INFO_THROTTLE( @@ -1587,7 +1643,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including intersection!!"); + debug_print_lat("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( @@ -1597,43 +1653,42 @@ bool NormalLaneChange::getLaneChangePaths( if ( lane_change_parameters_->regulate_on_traffic_light && !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { - debug_print("Reject: regulate on traffic light!!"); + debug_print_lat("Reject: regulate on traffic light!!"); continue; } if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + get_current_lanes(), candidate_path.value().path, planner_data_, lane_change_info.length.sum())) { - debug_print("Ego is stopping near traffic light. Do not allow lane change"); + debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; } candidate_paths->push_back(*candidate_path); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects.target_lane, - lane_change_buffer, is_goal_in_route, *lane_change_parameters_, - lane_change_debug_.collision_check_objects)) { - debug_print( + !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)) { + debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); return false; } if (!check_safety) { - debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); + debug_print_lat("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } - const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( + const auto [is_safe, is_trailing_object] = isLaneChangePathSafe( *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects); if (is_safe) { - debug_print("ACCEPT!!!: it is valid and safe!"); + debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } - debug_print("Reject: sampled path is not safe."); + debug_print_lat("Reject: sampled path is not safe."); } } } @@ -1671,7 +1726,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return {}; @@ -1726,9 +1781,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::BasicPoint2d lc_start_point( lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - const auto target_lane_polygon = - lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1740,8 +1793,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; @@ -1781,8 +1832,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - lane_change_info, reference_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, reference_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); return terminal_lane_change_path; } @@ -1790,13 +1841,30 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; - const auto & current_lanes = status_.current_lanes; - const auto & target_lanes = status_.target_lanes; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + if (current_lanes.empty() || target_lanes.empty()) { + return {true, true}; + } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); CollisionCheckDebugMap debug_data; + + const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + + const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( + common_data_ptr_, path, filtered_objects.target_lane_leading, min_lc_length, debug_data); + + if (!has_passed_parked_objects) { + RCLCPP_DEBUG(logger_, "Lane change has been delayed."); + return {false, false}; + } + const auto safety_status = isLaneChangePathSafe( path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { @@ -1848,8 +1916,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.target_lanes)); + *route_handler, utils::extendLanes(route_handler, get_current_lanes()), + utils::extendLanes(route_handler, get_target_lanes())); 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); @@ -1878,13 +1946,13 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const return true; } -bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) +bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && - isAbleToStopSafely() && is_object_coming_from_rear) { + isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && + isAbleToStopSafely() && is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -1901,16 +1969,16 @@ bool NormalLaneChange::calcAbortPath() std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; - const auto reference_lanelets = selected_path.info.current_lanes; const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); + route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); const auto & lane_changing_path = selected_path.path; + const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( @@ -1999,7 +2067,7 @@ bool NormalLaneChange::calcAbortPath() auto reference_lane_segment = prev_module_output_.path; { // const auto terminal_path = - // calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + // calcTerminalLaneChangePath(reference_lanelets, get_target_lanes()); // if (terminal_path) { // reference_lane_segment = terminal_path->path; // } @@ -2034,16 +2102,18 @@ bool NormalLaneChange::calcAbortPath() } PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & collision_check_objects, + const LaneChangePath & lane_change_path, + const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - PathSafetyStatus path_safety_status; + constexpr auto is_safe = true; + constexpr auto is_object_behind_ego = true; - if (collision_check_objects.empty()) { + if (collision_check_objects.leading.empty() && collision_check_objects.trailing.empty()) { RCLCPP_DEBUG(logger_, "There is nothing to check."); - return path_safety_status; + return {is_safe, !is_object_behind_ego}; } const auto & path = lane_change_path.path; @@ -2052,11 +2122,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto current_twist = getEgoTwist(); if (path.points.empty()) { - path_safety_status.is_safe = false; - return path_safety_status; + return {!is_safe, !is_object_behind_ego}; } - const double & time_resolution = lane_change_parameters_->prediction_time_resolution; + const auto time_resolution = lane_change_parameters_->prediction_time_resolution; const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, @@ -2064,23 +2133,24 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); - const auto current_lanes = getCurrentLanes(); - - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lane_change_path.info.target_lanes, direction_, - lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); + const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current; + const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target; constexpr double collision_check_yaw_diff_threshold{M_PI}; - for (const auto & obj : collision_check_objects) { + const auto check_collision = [&](const ExtendedPredictedObject & obj) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; + const auto selected_rss_param = + (obj.initial_twist.twist.linear.x <= + lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh) + ? lane_change_parameters_->rss_params_for_parked + : rss_params; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, current_debug_data.second); @@ -2090,10 +2160,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( continue; } - const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_current_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon); const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2101,21 +2171,28 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( continue; } - is_safe = false; - path_safety_status.is_safe = false; utils::path_safety_checker::updateCollisionCheckDebugMap( - debug_data, current_debug_data, is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + debug_data, current_debug_data, !is_safe); + return !is_safe; } utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, is_safe); + return is_safe; + }; + + for (const auto & obj : collision_check_objects.trailing) { + if (!check_collision(obj)) { + return {!is_safe, is_object_behind_ego}; + } + } + + for (const auto & obj : collision_check_objects.leading) { + if (!check_collision(obj)) { + return {!is_safe, !is_object_behind_ego}; + } } - return path_safety_status; + return {is_safe, !is_object_behind_ego}; } // Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes @@ -2218,6 +2295,18 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan return is_vehicle_stuck; } +bool NormalLaneChange::is_valid_start_point( + const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const +{ + const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); + + const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; + const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target.value(); + + return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || + boost::geometry::covered_by(lc_start_point, target_lane_poly); +} + void NormalLaneChange::setStopPose(const Pose & stop_pose) { lane_change_stop_pose_ = stop_pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp new file mode 100644 index 0000000000000..ac205ceedb34b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -0,0 +1,57 @@ + +// Copyright 2024 TIER IV, Inc. +// +// 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 +#include + +namespace autoware::behavior_path_planner::utils::lane_change::calculation +{ +double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) +{ + const auto & lanes_ptr = common_data_ptr->lanes_ptr; + const auto & current_lanes = lanes_ptr->current; + const auto & current_pose = common_data_ptr->get_ego_pose(); + + return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose); +} + +double calc_dist_from_pose_to_terminal_end( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, + const Pose & src_pose) +{ + if (lanes.empty()) { + return 0.0; + } + + const auto & lanes_ptr = common_data_ptr->lanes_ptr; + const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); + + if (lanes_ptr->current_lane_in_goal_section) { + return utils::getSignedDistance(src_pose, goal_pose, lanes); + } + return utils::getDistanceToEndOfLane(src_pose, lanes); +} + +double calc_stopping_distance(const LCParamPtr & lc_param_ptr) +{ + // v^2 = u^2 + 2ad + const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity; + const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc; + const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc)); + + const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; + return std::max(min_back_dist, param_back_dist); +} +} // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 9a553cf97af5c..eb64493b0c728 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -101,27 +101,36 @@ MarkerArray createLaneChangingVirtualWallMarker( } MarkerArray showFilteredObjects( - const ExtendedPredictedObjects & current_lane_objects, - const ExtendedPredictedObjects & target_lane_objects, - const ExtendedPredictedObjects & other_lane_objects, const std::string & ns) + const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; - auto current_marker = - marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id); + auto current_marker = marker_utils::showFilteredObjects( + filtered_objects.current_lane, ns, colors::yellow(), update_id); update_id += static_cast(current_marker.markers.size()); - auto target_marker = - marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id); - update_id += static_cast(target_marker.markers.size()); - auto other_marker = - marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id); + auto target_leading_marker = marker_utils::showFilteredObjects( + filtered_objects.target_lane_leading, ns, colors::aqua(), update_id); + update_id += static_cast(target_leading_marker.markers.size()); + auto target_trailing_marker = marker_utils::showFilteredObjects( + filtered_objects.target_lane_trailing, ns, colors::blue(), update_id); + update_id += static_cast(target_trailing_marker.markers.size()); + auto other_marker = marker_utils::showFilteredObjects( + filtered_objects.other_lane, ns, colors::medium_orchid(), update_id); MarkerArray marker_array; - marker_array.markers.insert( - marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end()); - marker_array.markers.insert( - marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); - marker_array.markers.insert( - marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); + std::move( + current_marker.markers.begin(), current_marker.markers.end(), + std::back_inserter(marker_array.markers)); + std::move( + target_leading_marker.markers.begin(), target_leading_marker.markers.end(), + std::back_inserter(marker_array.markers)); + + std::move( + target_trailing_marker.markers.begin(), target_trailing_marker.markers.end(), + std::back_inserter(marker_array.markers)); + + std::move( + other_marker.markers.begin(), other_marker.markers.end(), + std::back_inserter(marker_array.markers)); return marker_array; } @@ -190,9 +199,7 @@ MarkerArray createDebugMarkerArray( "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); - add(showFilteredObjects( - debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, - debug_filtered_objects.other_lane, "object_filtered")); + add(showFilteredObjects(debug_filtered_objects, "object_filtered")); if (!debug_collision_check_object.empty()) { add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info")); 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 d989a1e6d301d..71e6b0bf6530e 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 @@ -290,21 +290,6 @@ lanelet::ConstLanelets getTargetNeighborLanes( return neighbor_lanes; } -lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType & type) -{ - const auto target_neighbor_lanelets = - utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); - if (target_neighbor_lanelets.empty()) { - return {}; - } - const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( - target_neighbor_lanelets, 0, std::numeric_limits::max()); - - return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); -} - bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) @@ -330,13 +315,12 @@ bool isPathInLanelets( } std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; - const auto & original_lanes = lane_change_info.current_lanes; - const auto & target_lanes = lane_change_info.target_lanes; const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; const auto lane_change_velocity = lane_change_info.velocity; @@ -393,9 +377,11 @@ std::optional constructCandidatePath( const bool enable_path_check_in_lanelet = false; // check candidate path is in lanelet + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; if ( enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, original_lanes, target_lanes)) { + !isPathInLanelets(shifted_path.path, current_lanes, target_lanes)) { return std::nullopt; } @@ -526,7 +512,7 @@ std::vector generateDrivableLanes( } std::vector generateDrivableLanes( - const std::vector original_drivable_lanes, const RouteHandler & route_handler, + const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) { const auto has_same_lane = @@ -888,7 +874,7 @@ bool isParkedObject( const auto most_left_lanelet_candidates = route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_left_sub_type = most_left_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_left_lanelet_candidates) { @@ -898,7 +884,7 @@ bool isParkedObject( } } bound = most_left_lanelet.leftBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_left_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } else { @@ -909,7 +895,7 @@ bool isParkedObject( most_right_road_lanelet.rightBound()); lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_right_sub_type = most_right_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_right_lanelet_candidates) { @@ -919,7 +905,7 @@ bool isParkedObject( } } bound = most_right_lanelet.rightBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_right_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } @@ -967,30 +953,30 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, +bool passed_parked_objects( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, CollisionCheckDebugMap & object_debug) { + const auto route_handler = *common_data_ptr->route_handler_ptr; + const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; - const auto & path = lane_change_path.path; - const auto & current_lanes = lane_change_path.info.current_lanes; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto current_lane_path = route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { - return false; + if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { + return true; } const auto leading_obj_idx = getLeadingStaticObjectIdx( route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); if (!leading_obj_idx) { - return false; + return true; } const auto & leading_obj = objects.at(*leading_obj_idx); @@ -998,11 +984,13 @@ bool passParkedObject( const auto leading_obj_poly = autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { - return false; + return true; } const auto & current_path_end = current_lane_path.points.back().point.pose.position; double min_dist_to_end_of_current_lane = std::numeric_limits::max(); + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); for (const auto & point : leading_obj_poly.outer()) { const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); const double dist = autoware::motion_utils::calcSignedArcLength( @@ -1020,10 +1008,10 @@ bool passParkedObject( if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { debug.second.unsafe_reason = "delay lane change"; utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return true; + return false; } - return false; + return true; } std::optional getLeadingStaticObjectIdx( @@ -1135,10 +1123,9 @@ ExtendedPredictedObject transform( } bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) + const std::vector & collided_polygons, + const std::optional & lanes_polygon) { - const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); - const auto is_in_lanes = [&](const auto & collided_polygon) { return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); }; @@ -1207,27 +1194,167 @@ double calcPhaseLength( return std::min(length_with_acceleration, length_with_max_velocity); } -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes) +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { + const auto & lanes = common_data_ptr->lanes_ptr; LanesPolygon lanes_polygon; lanes_polygon.current = - utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + lanes_polygon.target = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); - for (const auto & target_backward_lane : target_backward_lanes) { - auto lane_polygon = utils::lane_change::createPolygon( - target_backward_lane, 0.0, std::numeric_limits::max()); + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, + lc_param_ptr->lane_expansion_right_offset); + lanes_polygon.expanded_target = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); + + lanes_polygon.target_neighbor = *utils::lane_change::createPolygon( + lanes->target_neighbor, 0.0, std::numeric_limits::max()); + + lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); + for (const auto & preceding_lane : lanes->preceding_target) { + auto lane_polygon = + utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); if (lane_polygon) { - lanes_polygon.target_backward.push_back(*lane_polygon); + lanes_polygon.preceding_target.push_back(*lane_polygon); } } return lanes_polygon; } + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + if (current_lanes.empty() || target_lanes.empty()) { + return false; + } + const auto & prev_current_lanes = common_data_ptr->lanes_ptr->current; + const auto & prev_target_lanes = common_data_ptr->lanes_ptr->target; + if (prev_current_lanes.empty() || prev_target_lanes.empty()) { + return false; + } + + if ( + (prev_current_lanes.front().id() != current_lanes.front().id()) || + (prev_current_lanes.back().id() != prev_current_lanes.back().id())) { + return false; + } + return (prev_target_lanes.front().id() == target_lanes.front().id()) && + (prev_target_lanes.back().id() == prev_target_lanes.back().id()); +} + +Pose to_pose( + const autoware::universe_utils::Point2d & point, + const geometry_msgs::msg::Quaternion & orientation) +{ + Pose pose; + pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); + pose.orientation = orientation; + return pose; +} + +bool is_ahead_of_ego( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object) +{ + const auto & current_ego_pose = common_data_ptr->get_ego_pose(); + + const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + + const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( + path.points, current_ego_pose.position, obj_position); + const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; + const auto lon_dev = std::max( + ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); + + // we don't always have to check the distance accurately. + if (std::abs(dist_to_base_link) > lon_dev) { + return dist_to_base_link >= 0.0; + } + + const auto ego_polygon = getEgoCurrentFootprint(current_ego_pose, ego_info).outer(); + auto ego_min_dist_to_end = std::numeric_limits::max(); + for (const auto & ego_edge_point : ego_polygon) { + const auto ego_edge = + autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); + const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( + path.points, ego_edge, path.points.back().point.pose.position); + ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); + } + + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + auto current_min_dist_to_end = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( + path.points, obj_p, path.points.back().point.pose.position); + current_min_dist_to_end = std::min(dist_ego_to_obj, current_min_dist_to_end); + } + return ego_min_dist_to_end - current_min_dist_to_end >= 0.0; +} + +bool is_before_terminal( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & lanes_ptr = common_data_ptr->lanes_ptr; + const auto terminal_position = (lanes_ptr->current_lane_in_goal_section) + ? route_handler_ptr->getGoalPose().position + : path.points.back().point.pose.position; + double current_max_dist = std::numeric_limits::lowest(); + + const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto dist_to_base_link = + autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); + // we don't always have to check the distance accurately. + if (std::abs(dist_to_base_link) > object.shape.dimensions.x) { + return dist_to_base_link >= 0.0; + } + + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto dist_obj_to_terminal = + autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); + current_max_dist = std::max(dist_obj_to_terminal, current_max_dist); + } + return current_max_dist >= 0.0; +} + +double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose) +{ + lanelet::ConstLanelet closest_lanelet; + + if (!lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { + return autoware::universe_utils::deg2rad(180); + } + const auto closest_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, pose.position); + return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); +} + +ExtendedPredictedObjects transform_to_extended_objects( + const CommonDataPtr & common_data_ptr, const std::vector & objects, + const bool check_prepare_phase) +{ + ExtendedPredictedObjects extended_objects; + extended_objects.reserve(objects.size()); + + const auto & bpp_param = *common_data_ptr->bpp_param_ptr; + const auto & lc_param = *common_data_ptr->lc_param_ptr; + std::transform( + objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { + return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase); + }); + + return extended_objects; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug