From e9bc8747e3e934da070822f26153d4d8e251242b Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 15 Jan 2024 18:34:48 +0900 Subject: [PATCH 1/9] saved Signed-off-by: Fumiya Watanabe --- .../scene.hpp | 8 +- .../utils/base_class.hpp | 2 + .../utils/utils.hpp | 4 + .../src/interface.cpp | 6 +- .../src/scene.cpp | 153 ++++++++++++++++++ .../src/utils/utils.cpp | 8 + 6 files changed, 179 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 8d2e3b451fe98..a494740a6cf7f 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + PathWithLaneId getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; @@ -141,7 +143,11 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() const override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + TurnSignalInfo calcTurnSignalInfo() override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 15fe2f8d2b8d9..d406bf0403937 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -88,6 +88,8 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; + virtual PathWithLaneId getTerminalLaneChangePath() const = 0; + virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; 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 67506326d92aa..e16afcdbf19ec 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 @@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & reference_path, const double shift_length); +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length); + PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 41d6b7aa6fc19..2d2000893a0a4 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -123,16 +123,20 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { + module_type_->updateLaneChangeStatus(); + *prev_approved_path_ = getPreviousModuleOutput().path; module_type_->insertStopPoint( module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = *prev_approved_path_; + out.path = module_type_->getTerminalLaneChangePath(); out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); const auto & lane_change_debug = module_type_->getDebugData(); for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index b56e9e544ea8d..46c31074b4975 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -140,6 +140,24 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +PathWithLaneId NormalLaneChange::getTerminalLaneChangePath() const +{ + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); + return prev_module_path_; + } + + const auto terminal_path = + calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + if (!terminal_path) { + RCLCPP_WARN(logger_, "Terminal path not found!!!"); + return prev_module_path_; + } + + return terminal_path->path; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; @@ -1413,6 +1431,141 @@ bool NormalLaneChange::getLaneChangePaths( return false; } +std::optional NormalLaneChange::calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto current_lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + prev_module_path_.points, current_lane_terminal_point, + -(lane_change_buffer + next_lane_change_buffer)); + + if (!lane_changing_start_pose) { + RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!"); + return {}; + } + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose.value()); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!"); + return {}; + } + + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( + target_lanes, lane_changing_start_pose.value()); + + const auto [min_lateral_acc, max_lateral_acc] = + lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, + minimum_lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong..."); + return {}; + } + + const lanelet::BasicPoint2d lc_start_point( + lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); + + const auto target_lane_polygon = + lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; + lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = lane_changing_start_pose.value(); + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = max_lateral_acc; + lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; + + if (!is_valid_start_point) { + RCLCPP_WARN( + logger_, + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + return {}; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_change_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, + lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!"); + return {}; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_changing_start_pose.value(), target_segment.points.front().point.pose, + target_lane_reference_path, shift_length); + + auto reference_segment = prev_module_path_; + const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + reference_segment.points, reference_segment.points.front().point.pose.position, + lane_changing_start_pose->position); + utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + + const auto minimum_lane_change_path = utils::lane_change::constructCandidatePath( + lane_change_info, reference_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + return minimum_lane_change_path; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; 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 df73990b770a3..fbd092b9fded6 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -437,6 +437,14 @@ ShiftLine getLaneChangingShiftLine( const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; + return getLaneChangingShiftLine( + lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); +} + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ ShiftLine shift_line; shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; From 1e581f69ea6cfa509d3e9b9b87bbb89521bd4391 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 18 Jan 2024 19:07:34 +0900 Subject: [PATCH 2/9] feat(lane_change): fix drivable area info Signed-off-by: Fumiya Watanabe --- .../scene.hpp | 4 +-- .../utils/base_class.hpp | 2 +- .../src/interface.cpp | 7 ++-- .../src/scene.cpp | 33 ++++++++++++++++--- 4 files changed, 33 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index a494740a6cf7f..e4af48a71c8f3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -53,7 +53,7 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; - PathWithLaneId getTerminalLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; BehaviorModuleOutput generateOutput() override; @@ -147,7 +147,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - TurnSignalInfo calcTurnSignalInfo() override; + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index d406bf0403937..35d018ad3d58c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -88,7 +88,7 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; - virtual PathWithLaneId getTerminalLaneChangePath() const = 0; + virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; virtual bool isEgoOnPreparePhase() const = 0; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 2d2000893a0a4..27d97390ac250 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -130,11 +130,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = module_type_->getTerminalLaneChangePath(); - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); + out = module_type_->getTerminalLaneChangePath(); + module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 46c31074b4975..ec6f63721e933 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -140,22 +140,44 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } -PathWithLaneId NormalLaneChange::getTerminalLaneChangePath() const +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const { + BehaviorModuleOutput output; + const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); - return prev_module_path_; + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; } const auto terminal_path = calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); if (!terminal_path) { RCLCPP_WARN(logger_, "Terminal path not found!!!"); - return prev_module_path_; + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; } - return terminal_path->path; + output.path = terminal_path->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = updateOutputTurnSignal(); + + extendOutputDrivableArea(output); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; } BehaviorModuleOutput NormalLaneChange::generateOutput() @@ -1469,7 +1491,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); - const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( prev_module_path_.points, current_lane_terminal_point, -(lane_change_buffer + next_lane_change_buffer)); @@ -1558,6 +1580,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points, reference_segment.points.front().point.pose.position, lane_changing_start_pose->position); utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; const auto minimum_lane_change_path = utils::lane_change::constructCandidatePath( lane_change_info, reference_segment, target_segment, target_lane_reference_path, From 9fa11a16d3cc2a2af49330e538c6cedb2032c1d8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 25 Jan 2024 16:41:02 +0900 Subject: [PATCH 3/9] feat(lane_change): update filter object Signed-off-by: Fumiya Watanabe --- .../src/interface.cpp | 4 +-- .../src/scene.cpp | 28 +++++++++++++++++-- 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 27d97390ac250..2d6def37b1e29 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -126,12 +126,12 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() module_type_->updateLaneChangeStatus(); *prev_approved_path_ = getPreviousModuleOutput().path; - module_type_->insertStopPoint( - module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; out = module_type_->getTerminalLaneChangePath(); + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index ec6f63721e933..3e01a947ed2c4 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -875,6 +875,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); // Guard if (objects.objects.empty()) { @@ -944,6 +948,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }; + // ignore object that are ahead of terminal lane change start + { + double distance_to_terminal_from_object = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon_outer) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + Pose polygon_pose; + polygon_pose.position = obj_p; + polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); + if (dist < distance_to_terminal_from_object) { + distance_to_terminal_from_object = dist; + } + } + if (minimum_lane_change_length > distance_to_terminal_from_object) { + continue; + } + } + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -1580,13 +1602,15 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points, reference_segment.points.front().point.pose.position, lane_changing_start_pose->position); utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + // remove terminal points because utils::clipPathLenght() calculates extra long path + reference_segment.points.pop_back(); reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; - const auto minimum_lane_change_path = utils::lane_change::constructCandidatePath( + const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( lane_change_info, reference_segment, target_segment, target_lane_reference_path, sorted_lane_ids); - return minimum_lane_change_path; + return terminal_lane_change_path; } PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const From d9a06c09966c0ac0e2cbb9320a5fe1c0b34fa220 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 25 Jan 2024 17:49:01 +0900 Subject: [PATCH 4/9] feat(lane_change): fix turn signal Signed-off-by: Fumiya Watanabe --- .../behavior_path_lane_change_module/src/interface.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 2d6def37b1e29..3bd5a1929baed 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -129,8 +129,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() BehaviorModuleOutput out; out = module_type_->getTerminalLaneChangePath(); - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + out.turn_signal_info = + getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); @@ -439,16 +440,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; const double path_length = motion_utils::calcArcLength(path.points); - const auto & front_point = path.points.front().point.pose.position; const size_t & current_nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double length_front_to_ego = motion_utils::calcSignedArcLength( - path.points, front_point, static_cast(0), current_pose.position, - current_nearest_seg_idx); + const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); - if (path_length - length_front_to_ego < buffer && start_pose) { + if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; From bebc6ec739444e321b9ffc9f80c77b59d3071445 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 25 Jan 2024 18:18:53 +0900 Subject: [PATCH 5/9] fix(lane_change): fix typo Signed-off-by: Fumiya Watanabe --- planning/behavior_path_lane_change_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 3e01a947ed2c4..667c4fa8bb539 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1602,7 +1602,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points, reference_segment.points.front().point.pose.position, lane_changing_start_pose->position); utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); - // remove terminal points because utils::clipPathLenght() calculates extra long path + // remove terminal points because utils::clipPathLength() calculates extra long path reference_segment.points.pop_back(); reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; From d55def5be6d3afedfba702eaf70d1fb0f5d2803f Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 25 Jan 2024 18:22:40 +0900 Subject: [PATCH 6/9] fix(lane_change): remove updateLaneChangeStatus() Signed-off-by: Fumiya Watanabe --- planning/behavior_path_lane_change_module/src/interface.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 3bd5a1929baed..e12553a22fc00 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -123,8 +123,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - module_type_->updateLaneChangeStatus(); - *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out; From 0dec1cd9e2927eae3b5698c60d8a10809d0ec623 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 25 Jan 2024 18:25:13 +0900 Subject: [PATCH 7/9] fix(lane_change): remove redundant process Signed-off-by: Fumiya Watanabe --- planning/behavior_path_lane_change_module/src/interface.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index e12553a22fc00..9c04ff4c67c58 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -131,9 +131,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.turn_signal_info = getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - const auto & lane_change_debug = module_type_->getDebugData(); for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; From 48d535597af237b04c7f75464872245511684f67 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 26 Jan 2024 13:41:15 +0900 Subject: [PATCH 8/9] fix(lane_change): calculate distance to goal Signed-off-by: Fumiya Watanabe --- .../behavior_path_lane_change_module/src/scene.cpp | 11 +++++++++-- .../src/utils/utils.cpp | 8 ++++---- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 667c4fa8bb539..0cd2b4c7c9abe 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1513,9 +1513,16 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); - auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + + double distance_to_terminal_from_goal = 0; + if (is_goal_in_route) { + distance_to_terminal_from_goal = + utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); + } + + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( prev_module_path_.points, current_lane_terminal_point, - -(lane_change_buffer + next_lane_change_buffer)); + -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!"); 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 fbd092b9fded6..e6a162d2bdad5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -412,10 +412,6 @@ PathWithLaneId getReferencePathFromTargetLane( return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); }); - if (s_end - s_start < lane_changing_length) { - return PathWithLaneId(); - } - RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") @@ -423,6 +419,10 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); + if (s_end - s_start < lane_changing_length) { + return PathWithLaneId(); + } + const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); From 1dc6ee37f026069d620603f2c3c524d48f7671ea Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 26 Jan 2024 16:27:17 +0900 Subject: [PATCH 9/9] fix(lane_change): add empty guard Signed-off-by: Fumiya Watanabe --- planning/behavior_path_lane_change_module/src/scene.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 0cd2b4c7c9abe..c5a0f21d0f909 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1672,6 +1672,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const TurnSignalInfo turn_signal_info{}; + if (path.path.points.empty()) { + return turn_signal_info; + } + // desired start pose = prepare start pose turn_signal_info.desired_start_point = std::invoke([&]() { const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time;