Merge pull request #1193 from tier4/feat/steering_factor_for_avoidance

feat(avoidance): (TEMPORARY) steering factor for avoidance maneuver

Author: satoshi-ota

Diff for: planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -122,11 +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());
-      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, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
-      }
     }
 
     for (const auto & right_shift : right_shift_array_) {
@@ -136,11 +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());
-      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, SteeringFactor::RIGHT, SteeringFactor::TURNING, "");
-      }
     }
   }
 
@@ -417,6 +407,10 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool safe_{true};
 
+  bool is_recording_{false};
+
+  bool is_record_necessary_{false};
+
   std::shared_ptr<AvoidanceHelper> helper_;
 
   std::shared_ptr<AvoidanceParameters> parameters_;

Diff for: planning/behavior_path_avoidance_module/src/scene.cpp

@@ -97,6 +97,34 @@ lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
     bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); });
   return ret;
 }
+
+bool straddleRoadBound(
+  const ShiftedPath & path, const lanelet::ConstLanelets & lanes,
+  const vehicle_info_util::VehicleInfo & vehicle_info)
+{
+  using boost::geometry::intersects;
+  using tier4_autoware_utils::pose2transform;
+  using tier4_autoware_utils::transformVector;
+
+  const auto footprint = vehicle_info.createFootprint();
+
+  for (const auto & lane : lanes) {
+    for (const auto & p : path.path.points) {
+      const auto transform = pose2transform(p.point.pose);
+      const auto shifted_vehicle_footprint = transformVector(footprint, transform);
+
+      if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
+        return true;
+      }
+
+      if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
 }  // namespace
 
 AvoidanceModule::AvoidanceModule(
@@ -894,6 +922,48 @@ BehaviorModuleOutput AvoidanceModule::plan()
       planner_data_->parameters.ego_nearest_yaw_threshold);
   }
 
+  // check if the ego straddles lane border
+  if (!is_recording_) {
+    is_record_necessary_ = straddleRoadBound(
+      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(
@@ -974,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;
 }

Diff for: planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -286,6 +286,8 @@ class PlannerManager
 
     module_ptr->publishRTCStatus();
 
+    module_ptr->publishSteeringFactor();
+
     module_ptr->publishObjectsOfInterestMarker();
 
     processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true);