From f1f8029b62e59f17812d3190ee7bf0686b2e2c4a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 21 Jun 2024 17:59:05 +0900 Subject: [PATCH 01/19] refactor(lane_change): use lane change namespace for structs (#7508) * refactor(lane_change): use lane change namespace for structs Signed-off-by: Zulfaqar Azmi * Move lane change namespace to bottom level Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_lane_change_module/scene.hpp | 2 +- .../utils/base_class.hpp | 6 +++--- .../utils/data_structs.hpp | 11 ++--------- .../utils/debug_structs.hpp | 4 ++-- .../utils/markers.hpp | 2 +- .../behavior_path_lane_change_module/utils/utils.hpp | 4 ++-- 6 files changed, 11 insertions(+), 18 deletions(-) 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..be62492b7c2cc 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; 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..cb5746d60c1d3 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 @@ -130,7 +130,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; } @@ -242,7 +242,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/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..ffd2754acc38f 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 @@ -190,13 +190,6 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeTargetObjectIndices -{ - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; -}; - struct LaneChangeLanesFilteredObjects { utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; @@ -211,7 +204,7 @@ enum class LaneChangeModuleType { }; } // namespace autoware::behavior_path_planner -namespace autoware::behavior_path_planner::data::lane_change +namespace autoware::behavior_path_planner::lane_change { struct PathSafetyStatus { @@ -225,6 +218,6 @@ struct LanesPolygon std::optional target; std::vector target_backward; }; -} // namespace autoware::behavior_path_planner::data::lane_change +} // namespace autoware::behavior_path_planner::lane_change #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..1890b9f308e8e 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 @@ -71,6 +71,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..91d1f1db15cbc 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 @@ -30,7 +30,7 @@ namespace marker_utils::lane_change_markers { 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( 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..e25b67bdb73d8 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,8 @@ 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::LanesPolygon; +using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; From ccd4ffbb62af319d4732d6fbdb175fc5de70f06d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 24 Jun 2024 09:57:02 +0900 Subject: [PATCH 02/19] feat(route_handler): add unit test for lane change related functions (#7504) * RT1-6230 feat(route_handler): add unit test for lane change related functions Signed-off-by: Zulfaqar Azmi * fix spell check Signed-off-by: Zulfaqar Azmi * fix spellcheck Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/autoware_test_utils.cpp | 30 ++- .../test/test_route_handler.cpp | 238 +++++++++++++----- .../test/test_route_handler.hpp | 43 +++- 3 files changed, 234 insertions(+), 77 deletions(-) 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 From 1000fe036cf5ae2a83da0402bb0c065294e4a540 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 5 Jul 2024 11:37:50 +0900 Subject: [PATCH 03/19] refactor(lane_change): move struct to lane change namespace (#7841) * move struct to lane change namespace Signed-off-by: Muhammad Zulfaqar Azmi * Revert "move struct to lane change namespace" This reverts commit 306984a76103c427732f170a6f7eb5f94e895b0b. Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../utils/base_class.hpp | 15 ++- .../utils/data_structs.hpp | 102 ++++++++++++++---- .../utils/path.hpp | 4 +- 3 files changed, 96 insertions(+), 25 deletions(-) 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 cb5746d60c1d3..cbd3a81827948 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 @@ -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()) @@ -155,7 +156,18 @@ 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); + } + 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_->direction = direction_; + } void setTimeKeeper(const std::shared_ptr & time_keeper) { @@ -228,6 +240,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}; 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 ffd2754acc38f..2fdf7c6b550a3 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,28 @@ #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 -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 +80,7 @@ struct LateralAccelerationMap } }; -struct LaneChangeCancelParameters +struct CancelParameters { bool enable_on_prepare_phase{true}; bool enable_on_lane_changing_phase{false}; @@ -83,7 +95,7 @@ struct LaneChangeCancelParameters int unsafe_hysteresis_threshold{2}; }; -struct LaneChangeParameters +struct Parameters { // trajectory generation double backward_lane_length{200.0}; @@ -92,8 +104,8 @@ 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}; @@ -143,7 +155,7 @@ struct LaneChangeParameters utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort - LaneChangeCancelParameters cancel{}; + CancelParameters cancel{}; double finish_judge_lateral_threshold{0.2}; @@ -151,32 +163,32 @@ struct LaneChangeParameters 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 Info { - 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}; + 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}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; @@ -190,22 +202,19 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeLanesFilteredObjects +struct LanesObjects { - utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects target_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects other_lane{}; + ExtendedPredictedObjects current_lane{}; + ExtendedPredictedObjects target_lane{}; + ExtendedPredictedObjects other_lane{}; }; -enum class LaneChangeModuleType { +enum class ModuleType { NORMAL = 0, EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; -} // namespace autoware::behavior_path_planner -namespace autoware::behavior_path_planner::lane_change -{ struct PathSafetyStatus { bool is_safe{true}; @@ -218,6 +227,55 @@ struct LanesPolygon std::optional target; std::vector target_backward; }; + +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + +struct CommonData +{ + std::shared_ptr route_handler_ptr; + Odometry::ConstSharedPtr self_odometry_ptr; + std::shared_ptr bpp_param_ptr; + std::shared_ptr lc_param_ptr; + Lanes lanes; + 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); + } +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using CommonDataPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change +namespace autoware::behavior_path_planner +{ +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 LaneChangeLanesFilteredObjects = 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/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..97b5c621deea5 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 @@ -31,8 +31,8 @@ struct LaneChangePath { PathWithLaneId path{}; ShiftedPath shifted_path{}; - PathWithLaneId prev_path{}; - LaneChangeInfo info{}; + LaneChangeInfo info; + bool is_safe{false}; }; using LaneChangePaths = std::vector; From 2cd660176df341aa06e1c67641867693f1690a82 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 17:39:49 +0900 Subject: [PATCH 04/19] fix(autoware_behavior_path_lane_change_module): fix shadowVariable (#7964) fix:shadowVariable Signed-off-by: kobayu858 Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene.cpp | 32 +++++++++---------- .../src/utils/utils.cpp | 8 ++--- 2 files changed, 20 insertions(+), 20 deletions(-) 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..c26f54e905b47 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 @@ -1455,7 +1455,7 @@ 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) { + const auto debug_print_lat = [&](const auto & s) { RCLCPP_DEBUG_STREAM( logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); @@ -1477,7 +1477,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); 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; } @@ -1496,7 +1496,7 @@ bool NormalLaneChange::getLaneChangePaths( 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!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1506,7 +1506,7 @@ bool NormalLaneChange::getLaneChangePaths( 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; } @@ -1537,7 +1537,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; if (!is_valid_start_point) { - debug_print( + debug_print_lat( "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); continue; @@ -1551,7 +1551,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; } @@ -1563,12 +1563,12 @@ bool NormalLaneChange::getLaneChangePaths( 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 +1576,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 +1587,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,14 +1597,14 @@ 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_, 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); @@ -1614,14 +1614,14 @@ bool NormalLaneChange::getLaneChangePaths( 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( + 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; } @@ -1629,11 +1629,11 @@ bool NormalLaneChange::getLaneChangePaths( *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."); } } } 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..9568f803773c4 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 @@ -888,7 +888,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 +898,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 +909,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 +919,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; } } From 3e4d2d0310cf358ca9ab5797adeaf0f45a224642 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 12 Jul 2024 22:32:34 +0900 Subject: [PATCH 05/19] refactor(lane_change): update lanes and its polygons only when it's updated (#7989) * refactor(lane_change): compute lanes and polygon only when updated Signed-off-by: Zulfaqar Azmi * Revert accidental changesd This reverts commit cbfd9ae8a88b2d6c3b27b35c9a08bb824ecd5011. Signed-off-by: Zulfaqar Azmi * fix spell check Signed-off-by: Zulfaqar Azmi * Make a common getter for current lanes Signed-off-by: Zulfaqar Azmi * add target lanes getter Signed-off-by: Zulfaqar Azmi * some minor function refactoring Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene.cpp | 4 +- .../scene.hpp | 13 +- .../utils/base_class.hpp | 19 +- .../utils/data_structs.hpp | 40 ++-- .../utils/path.hpp | 24 +- .../utils/utils.hpp | 22 +- .../src/interface.cpp | 3 +- .../src/scene.cpp | 218 ++++++++++-------- .../src/utils/utils.cpp | 76 ++++-- 9 files changed, 246 insertions(+), 173 deletions(-) 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/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 be62492b7c2cc..dad96d5f7371a 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 @@ -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; @@ -105,8 +107,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; @@ -124,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase const LaneChangeLanesFilteredObjects & predicted_objects, const lanelet::ConstLanelets & current_lanes) const; - LaneChangeLanesFilteredObjects filterObjects( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + LaneChangeLanesFilteredObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; @@ -203,6 +201,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 cbd3a81827948..c1cc14d98a7c7 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 @@ -65,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; @@ -139,6 +141,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_; } @@ -163,9 +170,19 @@ class LaneChangeBase 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_; } @@ -211,8 +228,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( 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 2fdf7c6b550a3..1bb4dfdeb59dc 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 @@ -183,6 +183,13 @@ struct PhaseInfo } }; +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + struct Info { PhaseInfo longitudinal_acceleration{0.0, 0.0}; @@ -190,9 +197,6 @@ struct Info PhaseInfo duration{0.0, 0.0}; PhaseInfo length{0.0, 0.0}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - Pose lane_changing_start{}; Pose lane_changing_end{}; @@ -225,23 +229,26 @@ struct LanesPolygon { std::optional current; std::optional target; - std::vector target_backward; + std::optional expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; }; -struct Lanes -{ - lanelet::ConstLanelets current; - lanelet::ConstLanelets target; - 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 { - std::shared_ptr route_handler_ptr; + RouteHandlerPtr route_handler_ptr; Odometry::ConstSharedPtr self_odometry_ptr; - std::shared_ptr bpp_param_ptr; - std::shared_ptr lc_param_ptr; - Lanes lanes; + 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; } @@ -259,12 +266,7 @@ struct CommonData return std::hypot(x, y); } }; - -using RouteHandlerPtr = std::shared_ptr; -using BppParamPtr = std::shared_ptr; -using LCParamPtr = std::shared_ptr; using CommonDataPtr = std::shared_ptr; -using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change 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/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 97b5c621deea5..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{}; - LaneChangeInfo info; - bool is_safe{false}; + 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 e25b67bdb73d8..17eaceb055fc1 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,6 +48,7 @@ using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +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; @@ -114,8 +115,9 @@ bool isPathInLanelets( 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( @@ -177,10 +179,9 @@ bool isParkedObject( const double ratio_threshold); bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + 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 bool is_goal_in_route, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -195,7 +196,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 +297,11 @@ 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); } // 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..4823cb0bfec22 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(); 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 c26f54e905b47..659e5b78bf577 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 @@ -41,7 +41,7 @@ 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; @@ -54,6 +54,42 @@ 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; + + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( + *common_data_ptr_->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 +97,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(); - - if (current_lanes.empty()) { - return {false, false}; - } + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - 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 +129,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 +144,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 +167,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 +260,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_; @@ -275,7 +298,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() 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 +318,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 +339,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,15 +385,14 @@ 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_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; return boost::geometry::covered_by( lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); }); @@ -450,7 +472,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 +499,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 +521,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 +592,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 +607,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,8 +684,9 @@ 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); + const auto & target_lanes = get_target_lanes(); + const double dist_to_lane_change_end = + utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes()); double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer @@ -686,7 +704,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const 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 +721,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 +742,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; @@ -763,7 +781,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; @@ -963,8 +981,7 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( return target_objects; } -LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const { const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); @@ -983,6 +1000,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( return {}; } + const auto & current_lanes = get_current_lanes(); + + if (current_lanes.empty()) { + return {}; + } + filterAheadTerminalObjects(objects, current_lanes); if (objects.objects.empty()) { @@ -993,6 +1016,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( std::vector current_lane_objects; std::vector other_lane_objects; + const auto & target_lanes = get_target_lanes(); + + if (target_lanes.empty()) { + return {}; + } + filterObjectsByLanelets( objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, other_lane_objects); @@ -1133,9 +1162,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,12 +1179,7 @@ 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); @@ -1177,7 +1199,7 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) { + if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { target_lane_objects.push_back(object); continue; } @@ -1187,7 +1209,7 @@ void NormalLaneChange::filterObjectsByLanelets( 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); }); @@ -1377,14 +1399,14 @@ bool NormalLaneChange::getLaneChangePaths( 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); @@ -1514,9 +1536,7 @@ bool NormalLaneChange::getLaneChangePaths( 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 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) || @@ -1529,8 +1549,6 @@ 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; @@ -1559,8 +1577,8 @@ 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_lat("Reject: failed to generate candidate path!!"); @@ -1602,7 +1620,7 @@ bool NormalLaneChange::getLaneChangePaths( } 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_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; @@ -1610,10 +1628,10 @@ bool NormalLaneChange::getLaneChangePaths( 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)) { + !is_stuck && + utils::lane_change::passParkedObject( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer, + is_goal_in_route, 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."); @@ -1671,7 +1689,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 +1744,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 +1756,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 +1795,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,10 +1804,14 @@ 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; @@ -1848,8 +1866,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); @@ -1883,7 +1901,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) 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) && + isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && isAbleToStopSafely() && is_object_coming_from_rear) { current_lane_change_state_ = LaneChangeStates::Stop; return true; @@ -1901,16 +1919,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 +2017,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; // } @@ -2064,12 +2082,8 @@ 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}; @@ -2090,10 +2104,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( 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 9568f803773c4..20610380a0a5b 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 @@ -330,13 +330,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 +392,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; } @@ -968,17 +969,18 @@ bool isParkedObject( } bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + 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 bool is_goal_in_route, 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()); @@ -1135,10 +1137,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 +1208,60 @@ 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()); + + 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()); + + const auto & route_handler = *common_data_ptr->route_handler_ptr; + lanes_polygon.target_neighbor = + getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type); - 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()); + 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()); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From ab60a5b9add673d6187c12fd009ba6a497008d63 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 23 Jul 2024 11:40:00 +0900 Subject: [PATCH 06/19] fix(lane_change): delay lane change cancel (#8048) RT1-6955: delay lane change cancel Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../utils/utils.hpp | 4 ++-- .../src/scene.cpp | 20 +++++++++++++++---- .../src/utils/utils.cpp | 19 +++++++++--------- 3 files changed, 28 insertions(+), 15 deletions(-) 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 17eaceb055fc1..703d882794ee4 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 @@ -178,10 +178,10 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passParkedObject( +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, CollisionCheckDebugMap & object_debug); + CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 659e5b78bf577..7a4b72631629d 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 @@ -1628,10 +1628,9 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->push_back(*candidate_path); if ( - !is_stuck && - utils::lane_change::passParkedObject( - common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer, - is_goal_in_route, lane_change_debug_.collision_check_objects)) { + !is_stuck && !utils::lane_change::passed_parked_objects( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, + 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."); @@ -1815,6 +1814,19 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const 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, 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); { 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 20610380a0a5b..6ea280817f39b 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 @@ -968,10 +968,10 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passParkedObject( +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, CollisionCheckDebugMap & object_debug) + CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -979,20 +979,19 @@ bool passParkedObject( 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 = 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); @@ -1000,11 +999,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( @@ -1022,10 +1023,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( From 6d38b7f37f6ff6cbc384a221691db601469c112c Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 25 Jul 2024 12:14:25 +0900 Subject: [PATCH 07/19] fix(lane_change): filtering object ahead of terminal (#8093) * employ lanelet based filtering before distance based filtering Signed-off-by: Zulfaqar Azmi * use distance based to terminal check instead Signed-off-by: Zulfaqar Azmi * remove RCLCPP INFO Signed-off-by: Muhammad Zulfaqar Azmi * update flow chart Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../README.md | 84 +++++----------- .../scene.hpp | 6 +- .../utils/data_structs.hpp | 1 + .../utils/utils.hpp | 12 +++ .../src/scene.cpp | 99 ++++++------------- .../src/utils/utils.cpp | 80 +++++++++++++++ 6 files changed, 153 insertions(+), 129 deletions(-) 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..63f5a2ec05bf1 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 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 dad96d5f7371a..a1284f355a87d 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 @@ -128,12 +128,8 @@ class NormalLaneChange : public LaneChangeBase 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, + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, std::vector & current_lane_objects, std::vector & target_lane_objects, std::vector & other_lane_objects) const; 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 1bb4dfdeb59dc..495fe9012ecd0 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 @@ -185,6 +185,7 @@ struct PhaseInfo struct Lanes { + bool current_lane_in_goal_section{false}; lanelet::ConstLanelets current; lanelet::ConstLanelets target; std::vector preceding_target; 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 703d882794ee4..6dbed08ea651d 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 @@ -302,6 +302,18 @@ 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); } // 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/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 7a4b72631629d..0969103ff585b 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 @@ -83,8 +83,11 @@ void NormalLaneChange::update_lanes(const bool is_approved) 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->current_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(current_lanes.back()); common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + *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_); @@ -983,7 +986,6 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( LaneChangeLanesFilteredObjects 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; @@ -1006,12 +1008,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } - filterAheadTerminalObjects(objects, current_lanes); - - if (objects.objects.empty()) { - return {}; - } - std::vector target_lane_objects; std::vector current_lane_objects; std::vector other_lane_objects; @@ -1022,9 +1018,11 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } + const auto path = + route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + filterObjectsByLanelets( - objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, - other_lane_objects); + objects, path, current_lane_objects, target_lane_objects, other_lane_objects); const auto is_within_vel_th = [](const auto & object) -> bool { constexpr double min_vel_th = 1.0; @@ -1032,38 +1030,25 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); }; - 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(); - - 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; - }; - utils::path_safety_checker::filterObjects( target_lane_objects, [&](const PredictedObject & object) { - return (is_within_vel_th(object) || is_ahead_of_ego(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( - other_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); - }); + if (lane_change_parameters_->check_objects_on_other_lanes) { + utils::path_safety_checker::filterObjects( + other_lane_objects, [&](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); + 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; @@ -1119,42 +1104,15 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -void NormalLaneChange::filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) 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); - }); -} - void NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, std::vector & current_lane_objects, + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, + 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) { @@ -1199,7 +1157,14 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { + 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.expanded_target) && is_lateral_far && + is_before_terminal()) { target_lane_objects.push_back(object); continue; } 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 6ea280817f39b..2ef7bd0a41cc3 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 @@ -1216,6 +1216,7 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.current = utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + lanes_polygon.target = utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); @@ -1263,6 +1264,85 @@ bool is_same_lane_with_prev_iteration( 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; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From 8a3411a4d3bcf07259370a9ea362090ca4606cb5 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 26 Jul 2024 16:34:56 +0900 Subject: [PATCH 08/19] fix(autoware_behavior_path_lane_change_module): fix passedByValue (#8208) fix:passedByValue Signed-off-by: kobayu858 --- .../autoware/behavior_path_lane_change_module/utils/utils.hpp | 2 +- .../src/utils/utils.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 6dbed08ea651d..7bb6a06116a53 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 @@ -136,7 +136,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( 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 2ef7bd0a41cc3..94b477d3e5737 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 @@ -527,7 +527,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 = From 4a1864a4354813b5407c9c9957b8fda73690da78 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 16:22:40 +0900 Subject: [PATCH 09/19] fix(lane_change): relax finish judge (#8133) * fix(lane_change): relax finish judge Signed-off-by: Muhammad Zulfaqar Azmi * documentation update Signed-off-by: Zulfaqar Azmi * update readme explanations Signed-off-by: Zulfaqar Azmi * update config Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../README.md | 66 ++++++++++++++++++- .../config/lane_change.param.yaml | 5 +- .../utils/data_structs.hpp | 5 +- .../utils/utils.hpp | 2 + .../src/manager.cpp | 9 ++- .../src/scene.cpp | 31 ++++++--- .../src/utils/utils.cpp | 11 ++++ 7 files changed, 114 insertions(+), 15 deletions(-) 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 63f5a2ec05bf1..967e8352b830a 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 @@ -618,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 @@ -631,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 | @@ -647,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 | 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..77cf9b7b11cbe 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] @@ -109,7 +108,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/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 495fe9012ecd0..696cf4bd58d32 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 @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -109,7 +110,6 @@ struct Parameters 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 @@ -157,7 +157,10 @@ struct Parameters // abort 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}; 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 7bb6a06116a53..d566aceba413b 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 @@ -314,6 +314,8 @@ bool is_ahead_of_ego( 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); } // 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/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 22c93f7d4bfd5..16c4058bb3911 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 @@ -166,8 +166,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 +222,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")); 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 0969103ff585b..655fa4f8320e2 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 @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include #include @@ -689,21 +690,33 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; const auto & target_lanes = get_target_lanes(); const double dist_to_lane_change_end = - utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes()); - double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; + utils::getSignedDistance(current_pose, lane_change_end, target_lanes); - // 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 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; } 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 94b477d3e5737..915864c597882 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 @@ -1343,6 +1343,17 @@ bool is_before_terminal( } 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)); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From dde26ce72e73936b05fd3637f63bb3d204acef67 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 17:24:57 +0900 Subject: [PATCH 10/19] feat(lane_change): use different rss param to deal with parked vehicle (#8316) * different rss value for parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi * Documentation and config file update Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../README.md | 12 ++++++++++++ .../config/lane_change.param.yaml | 8 ++++++++ .../utils/data_structs.hpp | 1 + .../src/manager.cpp | 17 +++++++++++++++++ .../src/scene.cpp | 7 ++++++- 5 files changed, 44 insertions(+), 1 deletion(-) 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 967e8352b830a..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 @@ -773,6 +773,18 @@ The following parameters are used to judge lane change completion. | `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 77cf9b7b11cbe..7a9c466fec260 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 @@ -38,6 +38,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 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 696cf4bd58d32..7962c878d7d64 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 @@ -151,6 +151,7 @@ struct Parameters 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{}; 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 16c4058bb3911..f3f371ec02a9b 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( 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 655fa4f8320e2..eda0c4024a934 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 @@ -2082,9 +2082,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( 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); From 70425d535dc32299b79c04ec78e0542b2049be7f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 7 Aug 2024 12:07:45 +0900 Subject: [PATCH 11/19] refactor(lane_change): check start point directly after getting start point (#8357) * check start point directly after getting start point Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../scene.hpp | 18 ++++++++ .../src/scene.cpp | 46 ++++++++----------- 2 files changed, 38 insertions(+), 26 deletions(-) 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 a1284f355a87d..605e0499adcdb 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 @@ -184,6 +184,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( 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 eda0c4024a934..d80acf4ec18b2 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 @@ -392,16 +392,9 @@ void NormalLaneChange::insertStopPoint( 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 = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - 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); @@ -1424,6 +1417,12 @@ bool NormalLaneChange::getLaneChangePaths( 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( @@ -1510,16 +1509,6 @@ bool NormalLaneChange::getLaneChangePaths( 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_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) || - 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}; @@ -1532,13 +1521,6 @@ bool NormalLaneChange::getLaneChangePaths( 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_lat( - "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( @@ -2227,6 +2209,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; From 98c7b19330cb0e711c2b5f2e21d4a40e0ef89612 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 7 Aug 2024 15:39:26 +0900 Subject: [PATCH 12/19] refactor(lane_change): refactor debug print when computing paths (#8358) Refactor debug print Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) 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 d80acf4ec18b2..c83216457feba 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 @@ -1226,8 +1226,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 = @@ -1391,12 +1389,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, @@ -1412,6 +1404,12 @@ bool NormalLaneChange::getLaneChangePaths( 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; @@ -1453,13 +1451,8 @@ bool NormalLaneChange::getLaneChangePaths( } RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); + debug_print("Prepare path satisfy constraints"); for (const auto & lateral_acc : sample_lat_acc) { - const auto debug_print_lat = [&](const auto & s) { - RCLCPP_DEBUG_STREAM( - logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " - << sampled_longitudinal_acc << ", lat_acc = " << lateral_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 = @@ -1475,6 +1468,14 @@ bool NormalLaneChange::getLaneChangePaths( 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 (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; From a1d4bfef566bfe2bb864cbfa39110ab0cd72a53d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 7 Aug 2024 17:03:48 +0900 Subject: [PATCH 13/19] fix(lane_change): skip path computation if len exceed dist to terminal start (#8359) Skip computation if prepare length exceed distance to terminal start Signed-off-by: Zulfaqar Azmi --- .../CMakeLists.txt | 1 + .../utils/calculation.hpp | 45 +++++++++++++++++++ .../src/scene.cpp | 14 +++++- .../src/utils/calculation.cpp | 45 +++++++++++++++++++ 4 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp 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/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..421b54db9f67a --- /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,45 @@ +// 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; + +/** + * @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); +} // 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/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index c83216457feba..c8bd1f111a2ae 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" @@ -45,6 +46,7 @@ using utils::lane_change::calcMinimumLaneChangeLength; 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, @@ -1361,7 +1363,7 @@ 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); @@ -1402,6 +1404,16 @@ 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; + } + auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); const auto debug_print = [&](const auto & s) { 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..521c30d406e7a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -0,0 +1,45 @@ +// 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); +} +} // namespace autoware::behavior_path_planner::utils::lane_change::calculation From 2e4164c92b397b85b960a0bbea9a0c583b46f173 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 9 Aug 2024 17:37:36 +0900 Subject: [PATCH 14/19] fix(lane_change): skip generating path if lane changing path is too long (#8362) rework. skip lane changing for insufficeient distance in target lane Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../utils/data_structs.hpp | 1 + .../utils/utils.hpp | 4 -- .../src/scene.cpp | 49 ++++++++++--------- .../src/utils/utils.cpp | 20 +------- 4 files changed, 30 insertions(+), 44 deletions(-) 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 7962c878d7d64..155fbfdb535f9 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 @@ -191,6 +191,7 @@ struct Lanes { bool current_lane_in_goal_section{false}; lanelet::ConstLanelets current; + lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; std::vector preceding_target; }; 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 d566aceba413b..ec93a3c999152 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 @@ -106,10 +106,6 @@ 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); 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 c8bd1f111a2ae..dc4d1022615c6 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 @@ -87,6 +87,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) 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( @@ -1464,6 +1467,9 @@ bool NormalLaneChange::getLaneChangePaths( RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); 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); @@ -1474,11 +1480,6 @@ 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( @@ -1493,26 +1494,30 @@ bool NormalLaneChange::getLaneChangePaths( 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_lat("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); 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 915864c597882..1d8ffaa4b7b18 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) @@ -1227,9 +1212,8 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.expanded_target = utils::lane_change::createPolygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); - const auto & route_handler = *common_data_ptr->route_handler_ptr; - lanes_polygon.target_neighbor = - getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type); + 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) { From b8306187b35d5785b8bafcf42387ee02cf66528b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 13 Aug 2024 10:43:13 +0900 Subject: [PATCH 15/19] fix(lane_change): skip generating path if longitudinal distance difference is less than threshold (#8363) * fix when prepare length is insufficient Signed-off-by: Muhammad Zulfaqar Azmi * add reason for comparing prev_prep_diff with eps for lc_length_diff Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../config/lane_change.param.yaml | 5 +++++ .../utils/data_structs.hpp | 3 +++ .../src/manager.cpp | 7 ++++++ .../src/scene.cpp | 22 +++++++++++++++++++ 4 files changed, 37 insertions(+) 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 7a9c466fec260..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 @@ -26,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 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 155fbfdb535f9..9ef47485ec68c 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 @@ -124,6 +124,9 @@ struct Parameters 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}; 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 f3f371ec02a9b..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 @@ -354,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 dc4d1022615c6..c13d9df08d5f5 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 @@ -1417,6 +1417,13 @@ bool NormalLaneChange::getLaneChangePaths( 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) { @@ -1488,6 +1495,21 @@ bool NormalLaneChange::getLaneChangePaths( 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_lat("Reject: length of lane changing path is longer than length to goal!!"); From 8314f3820be73ef78443d615f0934e1b1d63d2d4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 13 Aug 2024 11:11:46 +0900 Subject: [PATCH 16/19] refactor(lane_change): separate leading and trailing objects (#8214) * refactor(lane_change): separate leading and trailing objects Signed-off-by: Muhammad Zulfaqar Azmi * Refactor to use common function Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 17 +- .../utils/base_class.hpp | 2 +- .../utils/data_structs.hpp | 41 ++++- .../utils/debug_structs.hpp | 5 +- .../utils/markers.hpp | 5 +- .../utils/utils.hpp | 4 + .../src/interface.cpp | 2 +- .../src/scene.cpp | 164 +++++++++--------- .../src/utils/markers.cpp | 45 +++-- .../src/utils/utils.cpp | 17 ++ 10 files changed, 182 insertions(+), 120 deletions(-) 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 605e0499adcdb..cc695f820ee38 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 @@ -82,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, @@ -120,19 +120,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; + FilteredByLanesExtendedObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; - void filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, - 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, @@ -170,7 +167,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; 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 c1cc14d98a7c7..ccc9258324ffa 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 @@ -99,7 +99,7 @@ class LaneChangeBase 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; 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 9ef47485ec68c..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 @@ -215,11 +215,33 @@ struct Info double terminal_lane_changing_velocity{0.0}; }; +template struct LanesObjects { - ExtendedPredictedObjects current_lane{}; - ExtendedPredictedObjects target_lane{}; - ExtendedPredictedObjects 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 TargetObjects +{ + ExtendedPredictedObjects leading; + ExtendedPredictedObjects trailing; + TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing) + : leading(std::move(leading)), trailing(std::move(trailing)) + { + } }; enum class ModuleType { @@ -231,7 +253,13 @@ enum class ModuleType { 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 @@ -280,12 +308,15 @@ using CommonDataPtr = std::shared_ptr; 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 LaneChangeLanesFilteredObjects = lane_change::LanesObjects; +using FilteredByLanesObjects = lane_change::LanesObjects>; +using FilteredByLanesExtendedObjects = lane_change::LanesObjects; using LateralAccelerationMap = lane_change::LateralAccelerationMap; } // 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/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 1890b9f308e8e..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 @@ -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(); 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 91d1f1db15cbc..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,6 +29,7 @@ namespace marker_utils::lane_change_markers { +using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; @@ -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/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index ec93a3c999152..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 @@ -312,6 +312,10 @@ bool is_before_terminal( 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 4823cb0bfec22..30ac0051e0339 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 @@ -276,7 +276,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/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index c13d9df08d5f5..8eae075df8b41 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 @@ -465,7 +465,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; @@ -973,32 +974,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 +FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const { 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); @@ -1019,10 +1020,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } - std::vector target_lane_objects; - std::vector current_lane_objects; - std::vector other_lane_objects; - const auto & target_lanes = get_target_lanes(); if (target_lanes.empty()) { @@ -1032,8 +1029,7 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const const auto path = route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - filterObjectsByLanelets( - objects, path, current_lane_objects, target_lane_objects, other_lane_objects); + auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); const auto is_within_vel_th = [](const auto & object) -> bool { constexpr double min_vel_th = 1.0; @@ -1042,14 +1038,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const }; utils::path_safety_checker::filterObjects( - target_lane_objects, [&](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; - }); + 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( - other_lane_objects, [&](const PredictedObject & object) { + 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; @@ -1057,34 +1051,27 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const } utils::path_safety_checker::filterObjects( - current_lane_objects, [&](const PredictedObject & 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; } @@ -1115,12 +1102,14 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -void NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, - std::vector & current_lane_objects, - std::vector & target_lane_objects, - std::vector & other_lane_objects) const +FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const { + std::vector target_lane_leading_objects; + std::vector target_lane_trailing_objects; + std::vector current_lane_objects; + std::vector other_lane_objects; + 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; @@ -1152,12 +1141,11 @@ void NormalLaneChange::filterObjectsByLanelets( 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 { @@ -1176,7 +1164,13 @@ void NormalLaneChange::filterObjectsByLanelets( if ( check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && is_before_terminal()) { - target_lane_objects.push_back(object); + 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; } @@ -1191,7 +1185,7 @@ void NormalLaneChange::filterObjectsByLanelets( // 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; } @@ -1203,6 +1197,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( @@ -1629,7 +1627,7 @@ bool NormalLaneChange::getLaneChangePaths( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects.target_lane, + 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 " @@ -1642,7 +1640,7 @@ bool NormalLaneChange::getLaneChangePaths( 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) { @@ -1820,7 +1818,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const 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, min_lc_length, debug_data); + 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."); @@ -1908,13 +1906,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(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_object_coming_from_rear) { + isAbleToStopSafely() && is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -2064,16 +2062,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; @@ -2082,11 +2082,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_, @@ -2099,7 +2098,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( 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); @@ -2132,21 +2131,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 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 1d8ffaa4b7b18..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 @@ -1338,6 +1338,23 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co 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 From 55e6408fefcc39d7363e0b96fe129c87bc681b1d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 15 Aug 2024 11:33:55 +0900 Subject: [PATCH 17/19] fix(lane_change): fix invalid doesn't have stop point (#8470) fix invalid doesn't have stop point Signed-off-by: Zulfaqar Azmi --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 1 + 1 file changed, 1 insertion(+) 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 8eae075df8b41..062b74e4525fc 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 @@ -301,6 +301,7 @@ 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_; } From a05034fa0967b3a3f93198ce89c81c357caea454 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 15 Aug 2024 16:20:12 +0900 Subject: [PATCH 18/19] fix(lane_change): do not cancel when approaching terminal start (#8381) * do not cancel if ego vehicle approaching terminal start Signed-off-by: Muhammad Zulfaqar Azmi * Insert stop point if object is coming from rear Signed-off-by: Muhammad Zulfaqar Azmi * minor edit to fix conflict Signed-off-by: Muhammad Zulfaqar Azmi * rename function Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 2 ++ .../utils/base_class.hpp | 2 ++ .../utils/calculation.hpp | 18 +++++++++++++ .../src/interface.cpp | 9 +++++++ .../src/scene.cpp | 25 +++++++++++++++++++ .../src/utils/calculation.cpp | 12 +++++++++ 6 files changed, 68 insertions(+) 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 cc695f820ee38..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 @@ -92,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; 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 ccc9258324ffa..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 @@ -93,6 +93,8 @@ class LaneChangeBase virtual bool isAbleToReturnCurrentLane() const = 0; + virtual bool is_near_terminal() const = 0; + virtual LaneChangePath getLaneChangePath() const = 0; virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; 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 index 421b54db9f67a..3dc5e7ee62a57 100644 --- 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 @@ -19,6 +19,7 @@ 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. @@ -40,6 +41,23 @@ 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/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 30ac0051e0339..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 @@ -247,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"); 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 062b74e4525fc..a47faf8d3a215 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 @@ -769,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; 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 index 521c30d406e7a..ac205ceedb34b 100644 --- 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 @@ -1,3 +1,4 @@ + // Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -42,4 +43,15 @@ double calc_dist_from_pose_to_terminal_end( } 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 From c98677d68d10d8d84bd5a2b13d29049edac00a48 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 15 Aug 2024 16:49:37 +0900 Subject: [PATCH 19/19] fix(lane_change): moving object is filtered in the extended target lanes (#8218) * object 3rd Signed-off-by: Zulfaqar Azmi * named param Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) 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 a47faf8d3a215..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 @@ -1188,7 +1188,7 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( }; if ( - check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && + 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); @@ -1200,6 +1200,20 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( 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);