fix(avoidance): update logic

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -122,31 +122,6 @@ class AvoidanceModule : public SceneModuleInterface
         calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position);
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
         left_shift.uuid, true, start_distance, finish_distance, clock_->now());
-
-      const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted();
-      const auto record_end_time = is_recording_ && !helper_->isShifted();
-      const auto steering_factor = [&]() {
-        if (record_start_time) {
-          is_recording_ = true;
-          RCLCPP_DEBUG(getLogger(), "start left avoidance maneuver. get time stamp.");
-          return uint16_t(100);
-        }
-
-        if (record_end_time) {
-          is_recording_ = false;
-          is_record_necessary_ = false;
-          RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp.");
-          return uint16_t(200);
-        }
-
-        return SteeringFactor::LEFT;
-      }();
-
-      if (finish_distance > -1.0e-03) {
-        steering_factor_interface_ptr_->updateSteeringFactor(
-          {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
-          PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, "");
-      }
     }
 
     for (const auto & right_shift : right_shift_array_) {
@@ -156,31 +131,6 @@ class AvoidanceModule : public SceneModuleInterface
         calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position);
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
         right_shift.uuid, true, start_distance, finish_distance, clock_->now());
-
-      const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted();
-      const auto record_end_time = is_recording_ && !helper_->isShifted();
-      const auto steering_factor = [&]() {
-        if (record_start_time) {
-          is_recording_ = true;
-          RCLCPP_DEBUG(getLogger(), "start right avoidance maneuver. get time stamp.");
-          return uint16_t(100);
-        }
-
-        if (record_end_time) {
-          is_recording_ = false;
-          is_record_necessary_ = false;
-          RCLCPP_DEBUG(getLogger(), "end avoidance maneuver. get time stamp.");
-          return uint16_t(200);
-        }
-
-        return SteeringFactor::RIGHT;
-      }();
-
-      if (finish_distance > -1.0e-03) {
-        steering_factor_interface_ptr_->updateSteeringFactor(
-          {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
-          PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, "");
-      }
     }
   }
 

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -928,6 +928,42 @@ BehaviorModuleOutput AvoidanceModule::plan()
       spline_shift_path, data.current_lanelets, planner_data_->parameters.vehicle_info);
   }
 
+  if (!path_shifter_.getShiftLines().empty()) {
+    const auto front_shift_line = path_shifter_.getShiftLines().front();
+    const double start_distance = calcSignedArcLength(
+      data.reference_path.points, getEgoPosition(), front_shift_line.start.position);
+    const double finish_distance = calcSignedArcLength(
+      data.reference_path.points, getEgoPosition(), front_shift_line.end.position);
+
+    const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted();
+    const auto record_end_time = is_recording_ && !helper_->isShifted();
+    const auto relative_shift_length = front_shift_line.end_shift_length - helper_->getEgoShift();
+    const auto steering_direction =
+      relative_shift_length > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT;
+    const auto steering_status = [&]() {
+      if (record_start_time) {
+        is_recording_ = true;
+        RCLCPP_ERROR(getLogger(), "start right avoidance maneuver. get time stamp.");
+        return uint16_t(100);
+      }
+
+      if (record_end_time) {
+        is_recording_ = false;
+        is_record_necessary_ = false;
+        RCLCPP_ERROR(getLogger(), "end avoidance maneuver. get time stamp.");
+        return uint16_t(200);
+      }
+
+      return helper_->isShifted() ? SteeringFactor::TURNING : SteeringFactor::APPROACHING;
+    }();
+
+    if (finish_distance > -1.0e-03) {
+      steering_factor_interface_ptr_->updateSteeringFactor(
+        {front_shift_line.start, front_shift_line.end}, {start_distance, finish_distance},
+        PlanningBehavior::AVOIDANCE, steering_direction, steering_status, "");
+    }
+  }
+
   // sparse resampling for computational cost
   {
     spline_shift_path.path = utils::resamplePathWithSpline(
@@ -1008,14 +1044,6 @@ CandidateOutput AvoidanceModule::planCandidate() const
   output.start_distance_to_path_change = sl_front.start_longitudinal;
   output.finish_distance_to_path_change = sl_back.end_longitudinal;
 
-  const uint16_t steering_factor_direction = std::invoke([&output]() {
-    return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT;
-  });
-  steering_factor_interface_ptr_->updateSteeringFactor(
-    {sl_front.start, sl_back.end},
-    {output.start_distance_to_path_change, output.finish_distance_to_path_change},
-    PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, "");
-
   output.path_candidate = shifted_path.path;
   return output;
 }