From 78080176858c986393860be9a0c20629d48484ea Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 1 Mar 2024 13:47:04 +0900 Subject: [PATCH 01/19] wip add function to check if other vehicles can pass Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 2 + .../src/start_planner_module.cpp | 43 +++++++++++++++++++ 2 files changed, 45 insertions(+) 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 1368124929367..a80a7dfad0a83 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 @@ -208,6 +208,8 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; bool isOverlapWithCenterLane() const; + bool isPreventingRearVehicleFromCrossing() 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 3c08d62f500ae..132d7315613e7 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 @@ -39,6 +39,7 @@ #include using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -219,6 +220,7 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { + isPreventingRearVehicleFromCrossing(); return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && !isOverlapWithCenterLane(); } @@ -287,6 +289,47 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } +bool StartPlannerModule::isPreventingRearVehicleFromCrossing() 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 = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + + auto calc_right_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + std::vector boundary_path; + std::for_each( + boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { + const double x = boundary_point.x(); + const double y = boundary_point.y(); + boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + }); + + return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); + }; + + auto calc_left_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + return -calc_right_lateral_offset(boundary_line, search_pose); + }; + + for (const auto & current_lane : current_lanes) { + const lanelet::ConstLineString2d current_left_bound = current_lane.leftBound2d(); + const lanelet::ConstLineString2d current_right_bound = current_lane.rightBound2d(); + const double current_left_dist = calc_left_lateral_offset(current_left_bound, current_pose); + const double current_right_dist = calc_right_lateral_offset(current_right_bound, current_pose); + const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); + + std::cerr << "current_lane_id" << current_lane.id() << " current_lane_width " + << current_lane_width << "\n"; + } + return false; +} + bool StartPlannerModule::isOverlapWithCenterLane() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; From 8681a9fca8a65a89f839b8404e3bcfebacef171a Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 28 Feb 2024 15:54:46 +0900 Subject: [PATCH 02/19] further refactoring Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 16 ++++++++-------- 1 file changed, 8 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 132d7315613e7..b1be36f61c715 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 @@ -1191,14 +1191,14 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; - } + turn_signal.turn_signal.command = std::invoke([&]() { + if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE; + if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset) + return TurnIndicatorsCommand::ENABLE_RIGHT; + if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) + return TurnIndicatorsCommand::ENABLE_LEFT; + return TurnIndicatorsCommand::DISABLE; + }); turn_signal.desired_start_point = start_pose; turn_signal.required_start_point = start_pose; From bafcae39d6f3a50666a99823b9f8fdb4ac6ce9c2 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 1 Mar 2024 11:31:45 +0900 Subject: [PATCH 03/19] change breaks for return Signed-off-by: Daniel Sanchez --- .../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 b1be36f61c715..9779f532ea15a 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 @@ -1418,7 +1418,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - break; + return; } default: { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1432,7 +1432,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons ? utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) : current_drivable_area_info; - break; + return; } } } From 8bb563ec603064c707d4275a00bcdbd61d055989 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 1 Mar 2024 17:39:58 +0900 Subject: [PATCH 04/19] added way to check closest overhang point to target centerline Signed-off-by: Daniel Sanchez --- .../utils/utils.hpp | 4 - .../src/scene.cpp | 2 +- .../src/utils/utils.cpp | 37 ----- .../utils/utils.hpp | 4 + .../src/utils/utils.cpp | 37 +++++ .../src/start_planner_module.cpp | 131 +++++++++++++----- 6 files changed, 142 insertions(+), 73 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 46b47de128bd9..35cc02e867557 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( const Pose & curent_pose, const double abort_return_dist, const LaneChangeParameters & lane_change_parameters, const Direction direction); -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length); - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2021630f1ae34..1e97017b24856 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -843,7 +843,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = utils::lane_change::getBackwardLanelets( + const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 48a5a8722b0e9..bae733610905b 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( return true; } -// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length) -{ - if (target_lanes.empty()) { - return {}; - } - - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); - - if (arc_length.length >= backward_length) { - return {}; - } - - const auto & front_lane = target_lanes.front(); - const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( - front_lane, std::abs(backward_length - arc_length.length), {front_lane}); - - lanelet::ConstLanelets backward_lanes{}; - const auto num_of_lanes = std::invoke([&preceding_lanes]() { - size_t sum{0}; - for (const auto & lanes : preceding_lanes) { - sum += lanes.size(); - } - return sum; - }); - - backward_lanes.reserve(num_of_lanes); - - for (const auto & lanes : preceding_lanes) { - backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); - } - - return backward_lanes; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index dcee7696933dd..22db593b45586 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane( lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length); + lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 308cf9c4fed2c..6fcfad2209815 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath( return lanes; } +// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length) +{ + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto & front_lane = target_lanes.front(); + const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); + + lanelet::ConstLanelets backward_lanes{}; + const auto num_of_lanes = std::invoke([&preceding_lanes]() { + size_t sum{0}; + for (const auto & lanes : preceding_lanes) { + sum += lanes.size(); + } + return sum; + }); + + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : preceding_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); + } + + return backward_lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) 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 9779f532ea15a..b0076e2bcfd94 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 @@ -291,42 +291,111 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto current_lanes = utils::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + // const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + // const double backward_path_length = + // planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto target_lanes = utils::getCurrentLanes(planner_data_); + + if (target_lanes.empty()) return false; + + // auto calc_right_lateral_offset = [&]( + // const lanelet::ConstLineString2d & boundary_line, + // const geometry_msgs::msg::Pose & search_pose) { + // std::vector boundary_path; + // std::for_each( + // boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { + // const double x = boundary_point.x(); + // const double y = boundary_point.y(); + // boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + // }); + + // return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); + // }; + + // auto calc_left_lateral_offset = [&]( + // const lanelet::ConstLineString2d & boundary_line, + // const geometry_msgs::msg::Pose & search_pose) { + // return -calc_right_lateral_offset(boundary_line, search_pose); + // }; + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto vehicle_footprint = transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); - auto calc_right_lateral_offset = [&]( - const lanelet::ConstLineString2d & boundary_line, - const geometry_msgs::msg::Pose & search_pose) { - std::vector boundary_path; - std::for_each( - boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { - const double x = boundary_point.x(); - const double y = boundary_point.y(); - boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); - }); - - return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); - }; - - auto calc_left_lateral_offset = [&]( - const lanelet::ConstLineString2d & boundary_line, - const geometry_msgs::msg::Pose & search_pose) { - return -calc_right_lateral_offset(boundary_line, search_pose); - }; - - for (const auto & current_lane : current_lanes) { - const lanelet::ConstLineString2d current_left_bound = current_lane.leftBound2d(); - const lanelet::ConstLineString2d current_right_bound = current_lane.rightBound2d(); - const double current_left_dist = calc_left_lateral_offset(current_left_bound, current_pose); - const double current_right_dist = calc_right_lateral_offset(current_right_bound, current_pose); - const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); - - std::cerr << "current_lane_id" << current_lane.id() << " current_lane_width " - << current_lane_width << "\n"; + const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( + *route_handler, target_lanes, current_pose, 200.0); + + const auto centerline_path = + route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + + double smallest_lateral_offset = std::numeric_limits::max(); + geometry_msgs::msg::Pose ego_overhang_point; + for (const auto & point : vehicle_footprint) { + geometry_msgs::msg::Pose point_pose; + point_pose.position.x = point.x(); + point_pose.position.y = point.y(); + point_pose.position.z = 0.0; + + const auto nearest_index = motion_utils::findNearestIndex(centerline_path.points, point_pose); + if (!nearest_index) continue; + + const auto point_msg = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); + const auto lateral_offset = + std::abs(motion_utils::calcLateralOffset(centerline_path.points, point_msg)); + if (lateral_offset < smallest_lateral_offset) { + smallest_lateral_offset = lateral_offset; + ego_overhang_point.position.x = point.x(); + ego_overhang_point.position.y = point.y(); + ego_overhang_point.position.z = 0.0; + } } + + if (smallest_lateral_offset == std::numeric_limits::max()) return false; + std::cerr << "ego_overhang_point.position.x " << ego_overhang_point.position.x << "\n"; + std::cerr << "ego_overhang_point.position.y " << ego_overhang_point.position.y << "\n"; + std::cerr << "ego_overhang_point.position.z " << ego_overhang_point.position.z << "\n"; + std::cerr << "smallest_lateral_offset " << smallest_lateral_offset << "\n"; + + // // filtering objects with velocity, position and class + // const auto filtered_objects = utils::path_safety_checker::filterObjects( + // dynamic_object, route_handler, target_lanes, current_pose.position, + // objects_filtering_params_); + + // if (filtered_objects.objects.empty()) return false; + + // for (const auto & point : vehicle_footprint) { + // geometry_msgs::msg::Pose point_pose; + // point_pose.position.x = point.x(); + // point_pose.position.y = point.y(); + // point_pose.position.z = point.z(); + + // lanelet::Lanelet closest_lanelet; + // lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); + // lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + // // Check backwards just in case the Vehicle behind ego is in a different lanelet + // const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet); + // lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; + // relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); + + // for (const auto & current_lane : relevant_lanelets) { + // const lanelet::ConstLineString2d current_left_bound = current_lane.leftBound2d(); + // const lanelet::ConstLineString2d current_right_bound = current_lane.rightBound2d(); + // const double current_left_dist = calc_left_lateral_offset(current_left_bound, + // current_pose); const double current_right_dist = + // calc_right_lateral_offset(current_right_bound, current_pose); + // const double current_lane_width = + // std::fabs(current_left_dist) + std::fabs(current_right_dist); + + // // filtering objects based on the current position's lane + // const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + // target_lanes, route_handler, filtered_objects, objects_filtering_params_); + + // std::cerr << "current_lane_id " << current_lane.id() << " current_lane_width " + // << current_lane_width << "\n"; + // } + // } return false; } From 9b689a6088363c986162343e6b355a3497b8ab38 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 4 Mar 2024 09:09:32 +0900 Subject: [PATCH 05/19] wip get distance to left and right bounds Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 52 ++++++++++++------- 1 file changed, 33 insertions(+), 19 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 b0076e2bcfd94..8baaacc7cd593 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 @@ -300,25 +300,25 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const if (target_lanes.empty()) return false; - // auto calc_right_lateral_offset = [&]( - // const lanelet::ConstLineString2d & boundary_line, - // const geometry_msgs::msg::Pose & search_pose) { - // std::vector boundary_path; - // std::for_each( - // boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { - // const double x = boundary_point.x(); - // const double y = boundary_point.y(); - // boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); - // }); - - // return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); - // }; - - // auto calc_left_lateral_offset = [&]( - // const lanelet::ConstLineString2d & boundary_line, - // const geometry_msgs::msg::Pose & search_pose) { - // return -calc_right_lateral_offset(boundary_line, search_pose); - // }; + auto calc_right_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + std::vector boundary_path; + std::for_each( + boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { + const double x = boundary_point.x(); + const double y = boundary_point.y(); + boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + }); + + return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); + }; + + auto calc_left_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + return -calc_right_lateral_offset(boundary_line, search_pose); + }; const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto vehicle_footprint = @@ -331,6 +331,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); double smallest_lateral_offset = std::numeric_limits::max(); + double left_dist = 0; + double right_dist = 0; geometry_msgs::msg::Pose ego_overhang_point; for (const auto & point : vehicle_footprint) { geometry_msgs::msg::Pose point_pose; @@ -349,6 +351,16 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const ego_overhang_point.position.x = point.x(); ego_overhang_point.position.y = point.y(); ego_overhang_point.position.z = 0.0; + + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + + const lanelet::ConstLineString2d current_left_bound = closest_lanelet_const.leftBound2d(); + const lanelet::ConstLineString2d current_right_bound = closest_lanelet_const.rightBound2d(); + left_dist = calc_left_lateral_offset(current_left_bound, point_pose); + right_dist = calc_right_lateral_offset(current_right_bound, point_pose); + // const double current_lane_width = std::fabs(left_dist) + std::fabs(right_dist); } } @@ -357,6 +369,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const std::cerr << "ego_overhang_point.position.y " << ego_overhang_point.position.y << "\n"; std::cerr << "ego_overhang_point.position.z " << ego_overhang_point.position.z << "\n"; std::cerr << "smallest_lateral_offset " << smallest_lateral_offset << "\n"; + std::cerr << "left_dist " << left_dist << "\n"; + std::cerr << "right_dist " << right_dist << "\n"; // // filtering objects with velocity, position and class // const auto filtered_objects = utils::path_safety_checker::filterObjects( From 73925acaa5f1caeac750b125e11cfdb8089d0aa0 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 4 Mar 2024 18:02:13 +0900 Subject: [PATCH 06/19] add check for param dereferencing Signed-off-by: Daniel Sanchez --- .../src/utils/path_safety_checker/objects_filtering.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 188c50993e4ae..ea9872f268678 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -78,7 +78,7 @@ PredictedObjects filterObjects( const std::shared_ptr & params) { // Guard - if (objects->objects.empty()) { + if (objects->objects.empty() || !params) { return PredictedObjects(); } From 971e9b54a02d6333b94b30855c894b6dcea98f2a Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 4 Mar 2024 18:04:51 +0900 Subject: [PATCH 07/19] check gap and rear vehicle width Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 112 +++++++++--------- 1 file changed, 57 insertions(+), 55 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 8baaacc7cd593..4606181a012d8 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 @@ -220,9 +220,8 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { - isPreventingRearVehicleFromCrossing(); return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - !isOverlapWithCenterLane(); + !isPreventingRearVehicleFromCrossing(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -292,12 +291,9 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; - // const auto & dynamic_object = planner_data_->dynamic_object; + const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; - // const double backward_path_length = - // planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto target_lanes = utils::getCurrentLanes(planner_data_); - if (target_lanes.empty()) return false; auto calc_right_lateral_offset = [&]( @@ -333,24 +329,27 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const double smallest_lateral_offset = std::numeric_limits::max(); double left_dist = 0; double right_dist = 0; - geometry_msgs::msg::Pose ego_overhang_point; + geometry_msgs::msg::Pose ego_overhang_pose; for (const auto & point : vehicle_footprint) { geometry_msgs::msg::Pose point_pose; point_pose.position.x = point.x(); point_pose.position.y = point.y(); point_pose.position.z = 0.0; - const auto nearest_index = motion_utils::findNearestIndex(centerline_path.points, point_pose); - if (!nearest_index) continue; + const auto nearest_segment_index = + motion_utils::findNearestSegmentIndex(centerline_path.points, point_pose); + if (!nearest_segment_index) continue; const auto point_msg = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); - const auto lateral_offset = - std::abs(motion_utils::calcLateralOffset(centerline_path.points, point_msg)); + const auto lateral_offset = std::abs(motion_utils::calcLateralOffset( + centerline_path.points, point_msg, nearest_segment_index.value())); + if (std::isnan(lateral_offset)) continue; + if (lateral_offset < smallest_lateral_offset) { smallest_lateral_offset = lateral_offset; - ego_overhang_point.position.x = point.x(); - ego_overhang_point.position.y = point.y(); - ego_overhang_point.position.z = 0.0; + ego_overhang_pose.position.x = point.x(); + ego_overhang_pose.position.y = point.y(); + ego_overhang_pose.position.z = 0.0; lanelet::Lanelet closest_lanelet; lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); @@ -365,52 +364,55 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const } if (smallest_lateral_offset == std::numeric_limits::max()) return false; - std::cerr << "ego_overhang_point.position.x " << ego_overhang_point.position.x << "\n"; - std::cerr << "ego_overhang_point.position.y " << ego_overhang_point.position.y << "\n"; - std::cerr << "ego_overhang_point.position.z " << ego_overhang_point.position.z << "\n"; std::cerr << "smallest_lateral_offset " << smallest_lateral_offset << "\n"; std::cerr << "left_dist " << left_dist << "\n"; std::cerr << "right_dist " << right_dist << "\n"; + lanelet::Lanelet closest_lanelet; + const bool is_closest_lanelet = + lanelet::utils::query::getClosestLanelet(target_lanes, ego_overhang_pose, &closest_lanelet); + if (!is_closest_lanelet) return false; + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + // Check backwards just in case the Vehicle behind ego is in a different lanelet + const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet); + lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; + relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); + // // filtering objects with velocity, position and class - // const auto filtered_objects = utils::path_safety_checker::filterObjects( - // dynamic_object, route_handler, target_lanes, current_pose.position, - // objects_filtering_params_); - - // if (filtered_objects.objects.empty()) return false; - - // for (const auto & point : vehicle_footprint) { - // geometry_msgs::msg::Pose point_pose; - // point_pose.position.x = point.x(); - // point_pose.position.y = point.y(); - // point_pose.position.z = point.z(); - - // lanelet::Lanelet closest_lanelet; - // lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); - // lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); - // // Check backwards just in case the Vehicle behind ego is in a different lanelet - // const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet); - // lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; - // relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); - - // for (const auto & current_lane : relevant_lanelets) { - // const lanelet::ConstLineString2d current_left_bound = current_lane.leftBound2d(); - // const lanelet::ConstLineString2d current_right_bound = current_lane.rightBound2d(); - // const double current_left_dist = calc_left_lateral_offset(current_left_bound, - // current_pose); const double current_right_dist = - // calc_right_lateral_offset(current_right_bound, current_pose); - // const double current_lane_width = - // std::fabs(current_left_dist) + std::fabs(current_right_dist); - - // // filtering objects based on the current position's lane - // const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( - // target_lanes, route_handler, filtered_objects, objects_filtering_params_); - - // std::cerr << "current_lane_id " << current_lane.id() << " current_lane_width " - // << current_lane_width << "\n"; - // } - // } - return false; + std::cerr << "Filter objects \n"; + const auto filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, relevant_lanelets, current_pose.position, + objects_filtering_params_); + if (filtered_objects.objects.empty()) return false; + + // filtering objects based on the current position's lane + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + relevant_lanelets, route_handler, filtered_objects, objects_filtering_params_); + if (target_objects_on_lane.on_current_lane.empty()) return false; + + double arc_length_to_closet_object = std::numeric_limits::max(); + auto target_object_itr = target_objects_on_lane.on_current_lane.begin(); + for (auto itr = target_objects_on_lane.on_current_lane.begin(); + itr != target_objects_on_lane.on_current_lane.end(); itr++) { + const auto arc_length = motion_utils::calcSignedArcLength( + centerline_path.points, current_pose.position, itr->initial_pose.pose.position); + if (arc_length > 0.0) continue; // Object in front of ego is ignored + if (std::abs(arc_length) < std::abs(arc_length_to_closet_object)) { + arc_length_to_closet_object = arc_length; + target_object_itr = itr; + } + } + + if (arc_length_to_closet_object == std::numeric_limits::max()) return false; + const double remaining_gap = + (std::abs(left_dist) > std::abs(right_dist)) ? std::abs(left_dist) : std::abs(right_dist); + std::cerr << "Dims \n"; + std::cerr << "target_object_itr->shape.dimensions.y " << target_object_itr->shape.dimensions.y + << "\n"; + + std::cerr << "remaining_gap " << remaining_gap << "\n"; + + return target_object_itr->shape.dimensions.y < remaining_gap; } bool StartPlannerModule::isOverlapWithCenterLane() const From 8406b2e3bccaf929088571157554041535875983 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 08:28:17 +0900 Subject: [PATCH 08/19] bugfix boolean condition was inverted Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4606181a012d8..90b42dc2cad7d 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 @@ -412,7 +412,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const std::cerr << "remaining_gap " << remaining_gap << "\n"; - return target_object_itr->shape.dimensions.y < remaining_gap; + return target_object_itr->shape.dimensions.y > remaining_gap; } bool StartPlannerModule::isOverlapWithCenterLane() const From 51467c75aa98195ca9a24f1d6665ce758ec374f4 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 10:48:49 +0900 Subject: [PATCH 09/19] refactor Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 113 ++++++++++-------- 1 file changed, 61 insertions(+), 52 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 90b42dc2cad7d..dc202fea6c066 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 @@ -296,6 +296,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const const auto target_lanes = utils::getCurrentLanes(planner_data_); if (target_lanes.empty()) return false; + // Define functions to get distance between a point and a lane's boundaries. auto calc_right_lateral_offset = [&]( const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose) { @@ -316,20 +317,17 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const return -calc_right_lateral_offset(boundary_line, search_pose); }; + // Get the ego's overhang point closest to the centerline path and the gap between said point an + // the lane's border. const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto vehicle_footprint = transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); - - const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( - *route_handler, target_lanes, current_pose, 200.0); - const auto centerline_path = route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); - double smallest_lateral_offset = std::numeric_limits::max(); - double left_dist = 0; - double right_dist = 0; - geometry_msgs::msg::Pose ego_overhang_pose; + double left_dist_to_lane_border = 0.0; + double right_dist_to_lane_border = 0.0; + geometry_msgs::msg::Pose ego_overhang_point_as_pose; for (const auto & point : vehicle_footprint) { geometry_msgs::msg::Pose point_pose; point_pose.position.x = point.x(); @@ -347,9 +345,9 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const if (lateral_offset < smallest_lateral_offset) { smallest_lateral_offset = lateral_offset; - ego_overhang_pose.position.x = point.x(); - ego_overhang_pose.position.y = point.y(); - ego_overhang_pose.position.z = 0.0; + ego_overhang_point_as_pose.position.x = point.x(); + ego_overhang_point_as_pose.position.y = point.y(); + ego_overhang_point_as_pose.position.z = 0.0; lanelet::Lanelet closest_lanelet; lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); @@ -357,62 +355,73 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const const lanelet::ConstLineString2d current_left_bound = closest_lanelet_const.leftBound2d(); const lanelet::ConstLineString2d current_right_bound = closest_lanelet_const.rightBound2d(); - left_dist = calc_left_lateral_offset(current_left_bound, point_pose); - right_dist = calc_right_lateral_offset(current_right_bound, point_pose); - // const double current_lane_width = std::fabs(left_dist) + std::fabs(right_dist); + left_dist_to_lane_border = calc_left_lateral_offset(current_left_bound, point_pose); + right_dist_to_lane_border = calc_right_lateral_offset(current_right_bound, point_pose); } } - if (smallest_lateral_offset == std::numeric_limits::max()) return false; + + const double gap_between_ego_and_lane_border = + (std::abs(left_dist_to_lane_border) > std::abs(right_dist_to_lane_border)) + ? std::abs(left_dist_to_lane_border) + : std::abs(right_dist_to_lane_border); + std::cerr << "smallest_lateral_offset " << smallest_lateral_offset << "\n"; - std::cerr << "left_dist " << left_dist << "\n"; - std::cerr << "right_dist " << right_dist << "\n"; - - lanelet::Lanelet closest_lanelet; - const bool is_closest_lanelet = - lanelet::utils::query::getClosestLanelet(target_lanes, ego_overhang_pose, &closest_lanelet); - if (!is_closest_lanelet) return false; - lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); - // Check backwards just in case the Vehicle behind ego is in a different lanelet - const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet); - lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; - relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); - - // // filtering objects with velocity, position and class + std::cerr << "remaining gap " << gap_between_ego_and_lane_border << "\n"; + + // Get the lanelets that will be queried for target objects + const auto relevant_lanelets = std::invoke([&]() -> std::optional { + lanelet::Lanelet closest_lanelet; + const bool is_closest_lanelet = lanelet::utils::query::getClosestLanelet( + target_lanes, ego_overhang_point_as_pose, &closest_lanelet); + if (!is_closest_lanelet) return std::nullopt; + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + // Check backwards just in case the Vehicle behind ego is in a different lanelet + const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet); + // return all the relevant lanelets + lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; + relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); + return relevant_lanelets; + }); + if (!relevant_lanelets) return false; + + // filtering objects with velocity, position and class std::cerr << "Filter objects \n"; const auto filtered_objects = utils::path_safety_checker::filterObjects( - dynamic_object, route_handler, relevant_lanelets, current_pose.position, + dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, objects_filtering_params_); if (filtered_objects.objects.empty()) return false; // filtering objects based on the current position's lane const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( - relevant_lanelets, route_handler, filtered_objects, objects_filtering_params_); + relevant_lanelets.value(), route_handler, filtered_objects, objects_filtering_params_); if (target_objects_on_lane.on_current_lane.empty()) return false; - double arc_length_to_closet_object = std::numeric_limits::max(); - auto target_object_itr = target_objects_on_lane.on_current_lane.begin(); - for (auto itr = target_objects_on_lane.on_current_lane.begin(); - itr != target_objects_on_lane.on_current_lane.end(); itr++) { - const auto arc_length = motion_utils::calcSignedArcLength( - centerline_path.points, current_pose.position, itr->initial_pose.pose.position); - if (arc_length > 0.0) continue; // Object in front of ego is ignored - if (std::abs(arc_length) < std::abs(arc_length_to_closet_object)) { - arc_length_to_closet_object = arc_length; - target_object_itr = itr; - } - } - - if (arc_length_to_closet_object == std::numeric_limits::max()) return false; - const double remaining_gap = - (std::abs(left_dist) > std::abs(right_dist)) ? std::abs(left_dist) : std::abs(right_dist); - std::cerr << "Dims \n"; - std::cerr << "target_object_itr->shape.dimensions.y " << target_object_itr->shape.dimensions.y - << "\n"; + // Get the closest target obj in the relevant lanes + const auto closest_object_width = std::invoke([&]() -> std::optional { + double arc_length_to_closet_object = std::numeric_limits::max(); + double closest_object_width = -1.0; - std::cerr << "remaining_gap " << remaining_gap << "\n"; + std::for_each( + target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), + [&](const auto & o) { + const auto arc_length = motion_utils::calcSignedArcLength( + centerline_path.points, current_pose.position, o.initial_pose.pose.position); + if (arc_length > 0.0) return; + if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; + arc_length_to_closet_object = arc_length; + closest_object_width = o.shape.dimensions.y; + }); + if (closest_object_width < 0.0) return std::nullopt; + return closest_object_width; + }); + if (!closest_object_width) return false; - return target_object_itr->shape.dimensions.y > remaining_gap; + std::cerr << "Dims \n"; + std::cerr << "target_object_itr->shape.dimensions.y " << closest_object_width.value() << "\n"; + std::cerr << "gap_between_ego_and_lane_border " << gap_between_ego_and_lane_border << "\n"; + // Decide if the closest object does not fit in the gap left by the ego vehicle. + return closest_object_width.value() > gap_between_ego_and_lane_border; } bool StartPlannerModule::isOverlapWithCenterLane() const From d827c77d7c17384fdd3242960ad5ab4c2d3cff91 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 13:46:58 +0900 Subject: [PATCH 10/19] rename param Signed-off-by: Daniel Sanchez --- .../config/start_planner.param.yaml | 1 + .../behavior_path_start_planner_module/data_structs.hpp | 1 + planning/behavior_path_start_planner_module/src/manager.cpp | 6 ++++++ .../src/start_planner_module.cpp | 6 ++++-- 4 files changed, 12 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index fad29b84c9c76..b17db7f907471 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 + extra_width_margin_for_rear_obstacle: 0.5 th_moving_object_velocity: 1.0 object_types_to_check_for_path_generation: check_car: true diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 07c81d2edd050..1e32237ffd870 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -64,6 +64,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double extra_width_margin_for_rear_obstacle{0.0}; std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index beeb675efd3ae..050d591128a15 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_margin_from_front_object = @@ -371,6 +373,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam( + parameters, ns + "extra_width_margin_for_rear_obstacle", + p->extra_width_margin_for_rear_obstacle); + updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( 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 dc202fea6c066..c8b90d8b2eec2 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 @@ -401,7 +401,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); double closest_object_width = -1.0; - std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { @@ -420,8 +419,11 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const std::cerr << "Dims \n"; std::cerr << "target_object_itr->shape.dimensions.y " << closest_object_width.value() << "\n"; std::cerr << "gap_between_ego_and_lane_border " << gap_between_ego_and_lane_border << "\n"; + std::cerr << "parameters_->extra_width_margin_for_rear_obstacle " + << parameters_->extra_width_margin_for_rear_obstacle << "\n"; // Decide if the closest object does not fit in the gap left by the ego vehicle. - return closest_object_width.value() > gap_between_ego_and_lane_border; + return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > + gap_between_ego_and_lane_border; } bool StartPlannerModule::isOverlapWithCenterLane() const From 5e6f672d40715714c4bc7973bad5438410c06c9b Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 14:20:06 +0900 Subject: [PATCH 11/19] remove prints Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 10 ---------- 1 file changed, 10 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 c8b90d8b2eec2..106ec70c03369 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 @@ -366,9 +366,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const ? std::abs(left_dist_to_lane_border) : std::abs(right_dist_to_lane_border); - std::cerr << "smallest_lateral_offset " << smallest_lateral_offset << "\n"; - std::cerr << "remaining gap " << gap_between_ego_and_lane_border << "\n"; - // Get the lanelets that will be queried for target objects const auto relevant_lanelets = std::invoke([&]() -> std::optional { lanelet::Lanelet closest_lanelet; @@ -386,7 +383,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const if (!relevant_lanelets) return false; // filtering objects with velocity, position and class - std::cerr << "Filter objects \n"; const auto filtered_objects = utils::path_safety_checker::filterObjects( dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, objects_filtering_params_); @@ -415,12 +411,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const return closest_object_width; }); if (!closest_object_width) return false; - - std::cerr << "Dims \n"; - std::cerr << "target_object_itr->shape.dimensions.y " << closest_object_width.value() << "\n"; - std::cerr << "gap_between_ego_and_lane_border " << gap_between_ego_and_lane_border << "\n"; - std::cerr << "parameters_->extra_width_margin_for_rear_obstacle " - << parameters_->extra_width_margin_for_rear_obstacle << "\n"; // Decide if the closest object does not fit in the gap left by the ego vehicle. return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > gap_between_ego_and_lane_border; From b6cd6ed73ef2ff691dd8a5c0bdb1f0ba65f2764f Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 16:26:56 +0900 Subject: [PATCH 12/19] use a better function to get the previous lanelets Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 106ec70c03369..00713de46812d 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 @@ -374,7 +374,9 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const if (!is_closest_lanelet) return std::nullopt; lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); // Check backwards just in case the Vehicle behind ego is in a different lanelet - const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet); + constexpr double backwards_length = 200.0; + const auto prev_lanes = behavior_path_planner::utils::getBackwardLanelets( + *route_handler, target_lanes, current_pose, backwards_length); // return all the relevant lanelets lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); From 6d791b62788ce9de3b990187fe884bd03be792dd Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 16:41:42 +0900 Subject: [PATCH 13/19] update docs Signed-off-by: Daniel Sanchez --- .../README.md | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4b52aae91d032..1248dee407be3 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -319,27 +319,28 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | -| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | -| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | -| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | -| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | -| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | -| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | -| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | -| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | +| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | +| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ### **Ego vehicle's velocity planning** From 56239b0a71d4e479adaddc016d3434fc79ad35cd Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 6 Mar 2024 09:20:32 +0900 Subject: [PATCH 14/19] update description Signed-off-by: Daniel Sanchez --- planning/behavior_path_start_planner_module/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 1248dee407be3..7c4991259fa9f 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -222,7 +222,7 @@ This ordering is beneficial when the priority is to minimize the backward distan - **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) -- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles. +- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. - **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline. From 7e082b6a777e40f79c07a077c7f8ccfac34e02b0 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 7 Mar 2024 08:27:25 +0900 Subject: [PATCH 15/19] typo Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 00713de46812d..50fd809e5c7e2 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 @@ -317,7 +317,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const return -calc_right_lateral_offset(boundary_line, search_pose); }; - // Get the ego's overhang point closest to the centerline path and the gap between said point an + // Get the ego's overhang point closest to the centerline path and the gap between said point and // the lane's border. const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto vehicle_footprint = From 2bfd71e8bb0c3a4912cb9e186b3e5d2558b1c479 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 7 Mar 2024 08:33:19 +0900 Subject: [PATCH 16/19] update readme Signed-off-by: Daniel Sanchez --- planning/behavior_path_start_planner_module/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 7c4991259fa9f..ff0550e4d4867 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -224,7 +224,7 @@ This ordering is beneficial when the priority is to minimize the backward distan - **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. -- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline. +- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and that the rear vehicle can pass through the gap between the ego vehicle and the lane border. ```plantuml @startuml @@ -235,7 +235,7 @@ if (Collision with dynamic objects detected?) then (yes) if (Before departure?) then (yes) :Deactivate module decision is registered; else (no) - if (Can stop within constraints \n && \n no crossing centerline?) then (yes) + if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes) :Stop; else (no) :Continue with caution; @@ -250,7 +250,7 @@ stop #### **example of safety check performed range for shift pull out** -Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled. +Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint does not leave enough space for a rear vehicle to drive through, the safety check against dynamic objects is disabled.
![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100} From 766abe1f2501e4c759d501796e01d598b9c922a6 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 7 Mar 2024 11:03:51 +0900 Subject: [PATCH 17/19] Use the merging side to choose what lane bound to use Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 107 +++++++++--------- 2 files changed, 57 insertions(+), 52 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 a80a7dfad0a83..b20ebffdb0e60 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 @@ -208,7 +208,7 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; bool isOverlapWithCenterLane() const; - bool isPreventingRearVehicleFromCrossing() const; + bool isPreventingRearVehicleFromPassingThrough() const; bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() 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 50fd809e5c7e2..cd4c6c856fa63 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 @@ -221,7 +221,7 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - !isPreventingRearVehicleFromCrossing(); + !isPreventingRearVehicleFromPassingThrough(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -288,18 +288,20 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } -bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); + const auto target_lanes = utils::getCurrentLanes(planner_data_); if (target_lanes.empty()) return false; // Define functions to get distance between a point and a lane's boundaries. - auto calc_right_lateral_offset = [&]( - const lanelet::ConstLineString2d & boundary_line, - const geometry_msgs::msg::Pose & search_pose) { + auto calc_absolute_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { std::vector boundary_path; std::for_each( boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { @@ -311,60 +313,63 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); }; - auto calc_left_lateral_offset = [&]( - const lanelet::ConstLineString2d & boundary_line, - const geometry_msgs::msg::Pose & search_pose) { - return -calc_right_lateral_offset(boundary_line, search_pose); - }; + // Check from what side of the road the ego is merging + const auto centerline_path = + route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + const auto start_pose_nearest_segment_index = + motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); + if (!start_pose_nearest_segment_index) return false; + + const auto start_pose_point_msg = tier4_autoware_utils::createPoint( + start_pose.position.x, start_pose.position.y, start_pose.position.z); + const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( + centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); + if (std::isnan(starting_pose_lateral_offset)) return false; + + const bool ego_is_merging_from_the_left = (starting_pose_lateral_offset > 0.0); // Get the ego's overhang point closest to the centerline path and the gap between said point and // the lane's border. - const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); - const auto centerline_path = - route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); - double smallest_lateral_offset = std::numeric_limits::max(); - double left_dist_to_lane_border = 0.0; - double right_dist_to_lane_border = 0.0; - geometry_msgs::msg::Pose ego_overhang_point_as_pose; - for (const auto & point : vehicle_footprint) { - geometry_msgs::msg::Pose point_pose; - point_pose.position.x = point.x(); - point_pose.position.y = point.y(); - point_pose.position.z = 0.0; - - const auto nearest_segment_index = - motion_utils::findNearestSegmentIndex(centerline_path.points, point_pose); - if (!nearest_segment_index) continue; - - const auto point_msg = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); - const auto lateral_offset = std::abs(motion_utils::calcLateralOffset( - centerline_path.points, point_msg, nearest_segment_index.value())); - if (std::isnan(lateral_offset)) continue; - - if (lateral_offset < smallest_lateral_offset) { - smallest_lateral_offset = lateral_offset; - ego_overhang_point_as_pose.position.x = point.x(); - ego_overhang_point_as_pose.position.y = point.y(); - ego_overhang_point_as_pose.position.z = 0.0; + auto get_gap_between_ego_and_lane_border = + [&]( + geometry_msgs::msg::Pose & ego_overhang_point_as_pose, + const bool ego_is_merging_from_the_left) -> std::optional { + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); + for (const auto & point : vehicle_footprint) { + geometry_msgs::msg::Pose point_pose; + point_pose.position.x = point.x(); + point_pose.position.y = point.y(); + point_pose.position.z = 0.0; lanelet::Lanelet closest_lanelet; lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); - const lanelet::ConstLineString2d current_left_bound = closest_lanelet_const.leftBound2d(); - const lanelet::ConstLineString2d current_right_bound = closest_lanelet_const.rightBound2d(); - left_dist_to_lane_border = calc_left_lateral_offset(current_left_bound, point_pose); - right_dist_to_lane_border = calc_right_lateral_offset(current_right_bound, point_pose); + const lanelet::ConstLineString2d current_lane_bound = (ego_is_merging_from_the_left) + ? closest_lanelet_const.rightBound2d() + : closest_lanelet_const.leftBound2d(); + const double current_point_lateral_gap = + calc_absolute_lateral_offset(current_lane_bound, point_pose); + if (current_point_lateral_gap < smallest_lateral_gap_between_ego_and_border) { + smallest_lateral_gap_between_ego_and_border = current_point_lateral_gap; + ego_overhang_point_as_pose.position.x = point.x(); + ego_overhang_point_as_pose.position.y = point.y(); + ego_overhang_point_as_pose.position.z = 0.0; + } } - } - if (smallest_lateral_offset == std::numeric_limits::max()) return false; + if (smallest_lateral_gap_between_ego_and_border == std::numeric_limits::max()) { + return std::nullopt; + } + return smallest_lateral_gap_between_ego_and_border; + }; - const double gap_between_ego_and_lane_border = - (std::abs(left_dist_to_lane_border) > std::abs(right_dist_to_lane_border)) - ? std::abs(left_dist_to_lane_border) - : std::abs(right_dist_to_lane_border); + geometry_msgs::msg::Pose ego_overhang_point_as_pose; + const auto gap_between_ego_and_lane_border = + get_gap_between_ego_and_lane_border(ego_overhang_point_as_pose, ego_is_merging_from_the_left); + if (!gap_between_ego_and_lane_border) return false; // Get the lanelets that will be queried for target objects const auto relevant_lanelets = std::invoke([&]() -> std::optional { @@ -395,7 +400,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const relevant_lanelets.value(), route_handler, filtered_objects, objects_filtering_params_); if (target_objects_on_lane.on_current_lane.empty()) return false; - // Get the closest target obj in the relevant lanes + // Get the closest target obj width in the relevant lanes const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); double closest_object_width = -1.0; @@ -415,7 +420,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const if (!closest_object_width) return false; // Decide if the closest object does not fit in the gap left by the ego vehicle. return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > - gap_between_ego_and_lane_border; + gap_between_ego_and_lane_border.value(); } bool StartPlannerModule::isOverlapWithCenterLane() const From 887df43ae3db384c9032beba6057b71baf7add6a Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 7 Mar 2024 11:05:04 +0900 Subject: [PATCH 18/19] delete unused function Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 1 - .../src/start_planner_module.cpp | 31 ------------------- 2 files changed, 32 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 b20ebffdb0e60..6014e5b8f1fdc 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 @@ -207,7 +207,6 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; - bool isOverlapWithCenterLane() const; bool isPreventingRearVehicleFromPassingThrough() const; bool isCloseToOriginalStartPose() 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 cd4c6c856fa63..8b3e5932fce0c 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 @@ -423,37 +423,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const gap_between_ego_and_lane_border.value(); } -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 = - 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 = 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[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; - } - } - } - } - return false; -} - bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); From ec5179bdb1a38ff9d184caed1e20636e22e2e7ca Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 7 Mar 2024 11:55:26 +0900 Subject: [PATCH 19/19] add debug message Signed-off-by: Daniel Sanchez --- .../src/start_planner_module.cpp | 1 + 1 file changed, 1 insertion(+) 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 8b3e5932fce0c..88a33d05f4ea7 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 @@ -326,6 +326,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); if (std::isnan(starting_pose_lateral_offset)) return false; + RCLCPP_DEBUG(getLogger(), "starting pose lateral offset: %f", starting_pose_lateral_offset); const bool ego_is_merging_from_the_left = (starting_pose_lateral_offset > 0.0); // Get the ego's overhang point closest to the centerline path and the gap between said point and