From 8c18ea2a75ea9eb5af6ea08d61c430ae56a76a6e Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 1 Dec 2022 18:04:47 +0900 Subject: [PATCH] fix(behavior_path_planner): fix lane change path validation (#2421) * fix(behavior_path_planner): clip_path_for_goal Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): clip path for validation Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): use path point only to check lane departure Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): remove unnecessary change Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe Signed-off-by: kminoda --- .../lane_change/lane_change_module.hpp | 3 --- .../lane_change/lane_change_module.cpp | 19 +++++++++++++++---- .../src/scene_module/lane_change/util.cpp | 14 ++++++++++++-- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 5f48501f325b5..39de619c4e3eb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -20,7 +20,6 @@ #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" -#include "lane_departure_checker/lane_departure_checker.hpp" #include @@ -44,7 +43,6 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using lane_departure_checker::LaneDepartureChecker; using marker_utils::CollisionCheckDebug; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -105,7 +103,6 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneDepartureChecker lane_departure_checker_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 4c6cd3a2ca018..17199c68fd895 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -50,7 +50,6 @@ LaneChangeModule::LaneChangeModule( uuid_right_{generateUUID()} { steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); - lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); } BehaviorModuleOutput LaneChangeModule::run() @@ -146,6 +145,7 @@ BehaviorModuleOutput LaneChangeModule::plan() { constexpr double resample_interval{1.0}; auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval); + if (!isValidPath(path)) { status_.is_safe = false; return BehaviorModuleOutput{}; @@ -387,9 +387,20 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const drivable_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); const auto lanelets = util::transformToLanelets(expanded_lanes); - if (lane_departure_checker_.checkPathWillLeaveLane(lanelets, path)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; + + // check path points are in any lanelets + for (const auto & point : path.points) { + bool is_in_lanelet = false; + for (const auto & lanelet : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { + is_in_lanelet = true; + break; + } + } + if (!is_in_lanelet) { + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); + return false; + } } // check relative angle diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 7e3f55513e22e..9b3e65ec92a87 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -496,7 +496,12 @@ PathWithLaneId getReferencePathFromTargetLane( const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); const double & s_start = lane_change_start_arc_position.length; - const double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; + double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length); + } return route_handler.getCenterLinePath(target_lanes, s_start, s_end); } @@ -511,7 +516,12 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, in_target_end_pose); const double & s_start = lane_change_start_arc_position.length; - const double & s_end = lane_change_end_arc_position.length; + double s_end = lane_change_end_arc_position.length; + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length); + } return route_handler.getCenterLinePath(target_lanes, s_start, s_end); }