refactor(lane_change): refactor get_lane_change_lanes function (autow…

…arefoundation#9044)

* refactor(lane_change): refactor get_lane_change_lanes function

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Add doxygen comment for to_geom_msg_pose

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

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;
 

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -51,13 +51,16 @@ 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;
 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<lanelet::ConstLanelet> getLaneChangeTargetLane(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const LaneChangeModuleType type, const Direction & direction);
+
+std::optional<lanelet::ConstLanelet> get_lane_change_target_lane(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes);
 
 std::vector<PoseWithVelocityStamped> convertToPredictedPath(
   const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose,

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<PathWithLaneId> 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(

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<lanelet::ConstLanelet> getLaneChangeTargetLane(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const LaneChangeModuleType type, const Direction & direction)
+std::optional<lanelet::ConstLanelet> 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<PoseWithVelocityStamped> convertToPredictedPath(

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 <class LaneletPointType>
+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);

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);