From f6bbeedb2e0856c47e544b1c710167b256cb4488 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 30 Jan 2024 12:59:45 +0900 Subject: [PATCH 1/4] temp Signed-off-by: kyoichi-sugahara --- .../start_planner_module.hpp | 1 + .../src/start_planner_module.cpp | 37 +++++++++++++++++-- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index cffce8218c55..f8e95b7122d1 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -164,6 +164,7 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + bool isOverlapWithCenterLane() const; bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 8937c8a83769..1cc9fa63ca57 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -278,6 +278,35 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } +bool StartPlannerModule::isOverlapWithCenterLane() const +{ + const auto current_lanes = utils::getCurrentLanes(planner_data_); + const auto & vehicle_footprint = vehicle_info_.createFootprint(); + for (const auto & current_lane : current_lanes) { + std::vector centerline_points; + for (const auto & point : current_lane.centerline()) { + geometry_msgs::msg::Point center_point; + center_point.x = point.basicPoint().x(); + center_point.y = point.basicPoint().y(); + center_point.z = point.basicPoint().z(); + centerline_points.push_back(center_point); + } + for (size_t i = 0; i < centerline_points.size() - 1; ++i) { + const auto & p1 = centerline_points.at(i); + const auto & p2 = centerline_points.at(i + 1); + for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { + const auto & p3 = tier4_autoware_utils::toMsg(vehicle_footprint[i].to_3d()); + const auto & p4 = tier4_autoware_utils::toMsg(vehicle_footprint[i + 1].to_3d()); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + if (intersection.has_value()) { + return true; + } + } + } + } + return false; +} + bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -926,8 +955,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, std::numeric_limits::max()); - // Set the maximum backward distance less than the distance from the vehicle's base_link to the - // lane's rearmost point to prevent lane departure. + // Set the maximum backward distance less than the distance from the vehicle's base_link to + // the lane's rearmost point to prevent lane departure. const double current_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length; const double allowed_backward_distance = std::clamp( @@ -1215,8 +1244,8 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; - // Check if the goal and ego are in the same route segment. If not, this is out of scope of this - // function. Return false. + // Check if the goal and ego are in the same route segment. If not, this is out of scope of + // this function. Return false. lanelet::ConstLanelet ego_lanelet; rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet); const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet); From fd62172b009b840ccf9a0b34e70f88d069e8a0f8 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 30 Jan 2024 19:51:49 +0900 Subject: [PATCH 2/4] Add lanelet2_extension utility include and implement isOverlapWithCenterLane() function Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 1cc9fa63ca57..dd2a13edba07 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -21,6 +21,7 @@ #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include @@ -211,7 +212,8 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { - return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; + return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && + !isOverlapWithCenterLane(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -280,24 +282,26 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isOverlapWithCenterLane() const { + const Pose & current_pose = planner_data_->self_odometry->pose.pose; const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto & vehicle_footprint = vehicle_info_.createFootprint(); + const auto & local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto & vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); for (const auto & current_lane : current_lanes) { std::vector centerline_points; for (const auto & point : current_lane.centerline()) { - geometry_msgs::msg::Point center_point; - center_point.x = point.basicPoint().x(); - center_point.y = point.basicPoint().y(); - center_point.z = point.basicPoint().z(); + geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); centerline_points.push_back(center_point); } + for (size_t i = 0; i < centerline_points.size() - 1; ++i) { const auto & p1 = centerline_points.at(i); const auto & p2 = centerline_points.at(i + 1); for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { - const auto & p3 = tier4_autoware_utils::toMsg(vehicle_footprint[i].to_3d()); - const auto & p4 = tier4_autoware_utils::toMsg(vehicle_footprint[i + 1].to_3d()); + const auto & p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); + const auto & p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + if (intersection.has_value()) { return true; } From fe845a9ea7d92cff41e2e48a7a28869f4f1f6979 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 31 Jan 2024 17:31:56 +0900 Subject: [PATCH 3/4] Update planning/behavior_path_start_planner_module/src/start_planner_module.cpp Co-authored-by: Kosuke Takeuchi --- .../src/start_planner_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index dd2a13edba07..bcd50134d124 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -298,8 +298,8 @@ bool StartPlannerModule::isOverlapWithCenterLane() const const auto & p1 = centerline_points.at(i); const auto & p2 = centerline_points.at(i + 1); for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { - const auto & p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); - const auto & p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); + const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); + const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); if (intersection.has_value()) { From c0408f83c169167d3c99e5ee484b341418751d0f Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 31 Jan 2024 17:32:01 +0900 Subject: [PATCH 4/4] Update planning/behavior_path_start_planner_module/src/start_planner_module.cpp Co-authored-by: Kosuke Takeuchi --- .../src/start_planner_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index bcd50134d124..1ad25633c02a 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -284,8 +284,8 @@ bool StartPlannerModule::isOverlapWithCenterLane() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto & local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto & vehicle_footprint = + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); for (const auto & current_lane : current_lanes) { std::vector centerline_points;