refactor(autoware_behavior_path_side_shift_module): refactor shift le…

…ngth retrieval and improve path orientation handling (autowarefoundation#9539)

refactor(side_shift_module): refactor shift length retrieval and improve path orientation handling

Signed-off-by: Kyoichi Sugahara <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp

@@ -87,8 +87,6 @@ class SideShiftModule : public SceneModuleInterface
   // const methods
   void publishPath(const PathWithLaneId & path) const;
 
-  double getClosestShiftLength() const;
-
   // member
   PathWithLaneId refined_path_{};
   PathWithLaneId reference_path_{};

planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp

@@ -15,6 +15,8 @@
 #ifndef AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_
 
+#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
+
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -27,9 +29,24 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::TransformStamped;
 using tier4_planning_msgs::msg::PathWithLaneId;
 
+/**
+ * @brief Sets the orientation (yaw angle) for all points in the path.
+ * @param [in,out] path Path with lane ID to set orientation.
+ * @details For each point, calculates orientation based on:
+ *          - Vector to next point if not last point
+ *          - Vector from previous point if last point
+ *          - Zero angle if single point
+ */
 void setOrientation(PathWithLaneId * path);
 
-bool isAlmostZero(double v);
+/**
+ * @brief Gets the shift length at the closest path point to the ego position.
+ * @param [in] shifted_path Path with shift length information.
+ * @param [in] ego_point Current ego position.
+ * @return Shift length at the closest path point. Returns 0.0 if path is empty.
+ */
+double getClosestShiftLength(
+  const ShiftedPath & shifted_path, const geometry_msgs::msg::Point ego_point);
 
 }  // namespace autoware::behavior_path_planner
 

planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp

@@ -88,8 +88,7 @@ bool SideShiftModule::isExecutionRequested() const
   }
 
   // If the desired offset has a non-zero value, return true as we want to execute the plan.
-
-  const bool has_request = !isAlmostZero(requested_lateral_offset_);
+  const bool has_request = std::fabs(requested_lateral_offset_) >= 1.0e-4;
   RCLCPP_DEBUG_STREAM(
     getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request);
 
@@ -119,14 +118,15 @@ bool SideShiftModule::canTransitSuccessState()
 {
   // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original
   // drivable area,this module can stop the computation and return SUCCESS.
+  constexpr double ZERO_THRESHOLD = 1.0e-4;
 
   const auto isOffsetDiffAlmostZero = [this]() noexcept {
     const auto last_sp = path_shifter_.getLastShiftLine();
     if (last_sp) {
       const auto length = std::fabs(last_sp.value().end_shift_length);
       const auto lateral_offset = std::fabs(requested_lateral_offset_);
       const auto offset_diff = lateral_offset - length;
-      if (!isAlmostZero(offset_diff)) {
+      if (std::fabs(offset_diff) >= ZERO_THRESHOLD) {
         lateral_offset_change_request_ = true;
         return false;
       }
@@ -135,7 +135,7 @@ bool SideShiftModule::canTransitSuccessState()
   }();
 
   const bool no_offset_diff = isOffsetDiffAlmostZero;
-  const bool no_request = isAlmostZero(requested_lateral_offset_);
+  const bool no_request = std::fabs(requested_lateral_offset_) < ZERO_THRESHOLD;
 
   const auto no_shifted_plan = [&]() {
     if (prev_output_.shift_length.empty()) {
@@ -279,6 +279,11 @@ BehaviorModuleOutput SideShiftModule::plan()
   ShiftedPath shifted_path;
   path_shifter_.generate(&shifted_path);
 
+  if (shifted_path.path.points.empty()) {
+    RCLCPP_ERROR(getLogger(), "Generated shift_path has no points");
+    return {};
+  }
+
   // Reset orientation
   setOrientation(&shifted_path.path);
 
@@ -345,7 +350,8 @@ ShiftLine SideShiftModule::calcShiftLine() const
     std::max(p->min_distance_to_start_shifting, ego_speed * p->time_to_start_shifting);
 
   const double dist_to_end = [&]() {
-    const double shift_length = requested_lateral_offset_ - getClosestShiftLength();
+    const double shift_length =
+      requested_lateral_offset_ - getClosestShiftLength(prev_output_, getEgoPose().position);
     const double jerk_shifting_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk(
       shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed));
     const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance);
@@ -367,18 +373,6 @@ ShiftLine SideShiftModule::calcShiftLine() const
   return shift_line;
 }
 
-double SideShiftModule::getClosestShiftLength() const
-{
-  if (prev_output_.shift_length.empty()) {
-    return 0.0;
-  }
-
-  const auto ego_point = planner_data_->self_odometry->pose.pose.position;
-  const auto closest =
-    autoware::motion_utils::findNearestIndex(prev_output_.path.points, ego_point);
-  return prev_output_.shift_length.at(closest);
-}
-
 BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & path) const
 {
   BehaviorModuleOutput out;

planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp

@@ -26,12 +26,6 @@ namespace autoware::behavior_path_planner
 {
 void setOrientation(PathWithLaneId * path)
 {
-  if (!path) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("behavior_path_planner").get_child("side_shift").get_child("util"),
-      "Pointer to path is NULL!");
-  }
-
   // Reset orientation
   for (size_t idx = 0; idx < path->points.size(); ++idx) {
     double angle = 0.0;
@@ -53,9 +47,16 @@ void setOrientation(PathWithLaneId * path)
   }
 }
 
-bool isAlmostZero(double v)
+double getClosestShiftLength(
+  const ShiftedPath & shifted_path, const geometry_msgs::msg::Point ego_point)
 {
-  return std::fabs(v) < 1.0e-4;
+  if (shifted_path.shift_length.empty()) {
+    return 0.0;
+  }
+
+  const auto closest =
+    autoware::motion_utils::findNearestIndex(shifted_path.path.points, ego_point);
+  return shifted_path.shift_length.at(closest);
 }
 
 }  // namespace autoware::behavior_path_planner