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

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Mar 18, 2024 · 19d74b3 · 19d74b3
1 parent da02c2e
commit 19d74b3
Show file tree

Hide file tree

Showing 2 changed files with 80 additions and 2 deletions.
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
@@ -122,10 +122,30 @@ 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, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
+          PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, "");
       }
     }
 
@@ -136,10 +156,30 @@ 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, SteeringFactor::RIGHT, SteeringFactor::TURNING, "");
+          PlanningBehavior::AVOIDANCE, steering_factor, SteeringFactor::TURNING, "");
       }
     }
   }
@@ -417,6 +457,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 --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/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,12 @@ 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);
+  }
+
   // sparse resampling for computational cost
   {
     spline_shift_path.path = utils::resamplePathWithSpline(