From d4d40cd0b0896f6493b92faeb003183e73a83a9d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 7 Oct 2024 18:07:45 +0900 Subject: [PATCH] refactor(lane_change): refactor get_lane_change_lanes function (#9044) * refactor(lane_change): refactor get_lane_change_lanes function Signed-off-by: Zulfaqar Azmi * Add doxygen comment for to_geom_msg_pose Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 3 +- .../utils/base_class.hpp | 3 -- .../utils/utils.hpp | 9 ++-- .../src/scene.cpp | 43 +++++-------------- .../src/utils/utils.cpp | 28 ++++++------ .../utils/utils.hpp | 27 ++++++++++++ .../src/utils/utils.cpp | 21 +-------- 7 files changed, 63 insertions(+), 71 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 311063a8bbf4a..cc06d0b114d7b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -112,8 +112,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() const final; protected: - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const override; + lanelet::ConstLanelets get_lane_change_lanes(const lanelet::ConstLanelets & current_lanes) const; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 4816a2f6c4eac..c124353e2873a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -241,9 +241,6 @@ class LaneChangeBase virtual bool isAbleToStopSafely() const = 0; - virtual lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; - virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 8103434e34a61..3219dc7e777f1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -51,6 +51,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -58,6 +59,8 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; +bool is_mandatory_lane_change(const ModuleType lc_type); + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); @@ -118,9 +121,9 @@ double getLateralShift(const LaneChangePath & path); CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction); + +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes); std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 4360cc3f81807..5a9ee485320b8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -69,7 +69,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + const auto target_lanes = get_lane_change_lanes(current_lanes); if (target_lanes.empty()) { return; } @@ -345,7 +345,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const } const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); if (!terminal_path) { RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); return prev_module_output_; @@ -636,15 +636,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getGoalPose(); } - Pose back_pose; - const auto back_point = - lanelet::utils::conversion::toGeomMsgPt(next_lane.centerline2d().back()); - const double front_yaw = lanelet::utils::getLaneletAngle(next_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - return back_pose; + return utils::to_geom_msg_pose(next_lane.centerline2d().back(), next_lane); }); const auto dist_to_target_pose = @@ -689,8 +681,8 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const +lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes( + const lanelet::ConstLanelets & current_lanes) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { @@ -699,34 +691,21 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( // Get lane change lanes const auto & route_handler = getRouteHandler(); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction); + const auto lane_change_lane = + utils::lane_change::get_lane_change_target_lane(common_data_ptr_, current_lanes); if (!lane_change_lane) { return {}; } - const auto front_pose = std::invoke([&lane_change_lane]() { - const auto & p = lane_change_lane->centerline().front(); - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); - const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); - geometry_msgs::msg::Pose front_pose; - front_pose.position = front_point; - tf2::Quaternion quat; - quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(quat); - return front_pose; - }); - const auto forward_length = std::invoke([&]() { + const auto front_pose = + utils::to_geom_msg_pose(lane_change_lane->centerline().front(), *lane_change_lane); const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); const auto forward_path_length = planner_data_->parameters.forward_path_length; - if (signed_distance <= 0.0) { - return forward_path_length; - } - - return signed_distance + forward_path_length; + return forward_path_length + std::max(signed_distance, 0.0); }); + const auto backward_length = lane_change_parameters_->backward_lane_length; return route_handler->getLaneletSequence( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 98da9112bbc09..20bc982445b07 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -78,6 +78,12 @@ rclcpp::Logger get_logger() return logger; } +bool is_mandatory_lane_change(const ModuleType lc_type) +{ + return lc_type == LaneChangeModuleType::NORMAL || + lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; +} + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity) { @@ -160,14 +166,13 @@ lanelet::ConstLanelets getTargetNeighborLanes( lanelet::ConstLanelets neighbor_lanes; for (const auto & current_lane : current_lanes) { + const auto mandatory_lane_change = is_mandatory_lane_change(type); if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + if (mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } else { - if (type != LaneChangeModuleType::NORMAL) { + if (!mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } @@ -633,17 +638,16 @@ CandidateOutput assignToCandidate( return candidate_output; } -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction) +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { - return route_handler.getLaneChangeTarget(current_lanes, direction); + const auto direction = common_data_ptr->direction; + const auto route_handler_ptr = common_data_ptr->route_handler_ptr; + if (is_mandatory_lane_change(common_data_ptr->lc_type)) { + return route_handler_ptr->getLaneChangeTarget(current_lanes, direction); } - return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); + return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } std::vector convertToPredictedPath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 7ac9993ee8b01..2e13c75a46481 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -104,6 +104,33 @@ FrenetPoint convertToFrenetPoint( return frenet_point; } +/** + * @brief Converts a Lanelet point to a ROS Pose message. + * + * This function converts a point from a Lanelet map to a ROS geometry_msgs::msg::Pose. + * It sets the position from the point and calculates the orientation (yaw) based on the target + * lane. + * + * @tparam LaneletPointType The type of the input point. + * + * @param[in] src_point The point to convert. + * @param[in] target_lane The lanelet used to determine the orientation. + * + * @return A Pose message with the position and orientation of the point. + */ +template +Pose to_geom_msg_pose(const LaneletPointType & src_point, const lanelet::ConstLanelet & target_lane) +{ + const auto point = lanelet::utils::conversion::toGeomMsgPt(src_point); + const auto yaw = lanelet::utils::getLaneletAngle(target_lane, point); + geometry_msgs::msg::Pose pose; + pose.position = point; + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw); + pose.orientation = tf2::toMsg(quat); + return pose; +} + // distance (arclength) calculation double l2Norm(const Vector3 vector); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index a48e36e3599ac..f3f6df142185b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -896,25 +896,8 @@ double getArcLengthToTargetLanelet( const auto target_center_line = target_lane.centerline().basicLineString(); - Pose front_pose, back_pose; - - { - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.front()); - const double front_yaw = lanelet::utils::getLaneletAngle(target_lane, front_point); - front_pose.position = front_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(tf_quat); - } - - { - const auto back_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.back()); - const double back_yaw = lanelet::utils::getLaneletAngle(target_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, back_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - } + const auto front_pose = to_geom_msg_pose(target_center_line.front(), target_lane); + const auto back_pose = to_geom_msg_pose(target_center_line.back(), target_lane); const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose); const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose);