ttc param

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml

@@ -3,10 +3,12 @@
     blind_spot:
       use_pass_judge_line: true
       stop_line_margin: 1.0 # [m]
-      backward_length: 50.0 # [m]
+      backward_detection_length: 100.0 # [m]
       ignore_width_from_center_line: 0.7 # [m]
-      max_future_movement_time: 10.0 # [second]
-      threshold_yaw_diff: 0.523 # [rad]
       adjacent_extend_width: 1.5 # [m]
       opposite_adjacent_extend_width: 1.5 # [m]
+      max_future_movement_time: 10.0 # [second]
+      ttc_min: -5.0 # [s]
+      ttc_max: 5.0 # [s]
+      ttc_ego_minimal_velocity: 5.0 # [m/s]
       enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval

planning/behavior_velocity_blind_spot_module/src/manager.cpp

@@ -36,17 +36,20 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
   planner_param_.use_pass_judge_line =
     getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
   planner_param_.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
-  planner_param_.backward_length = getOrDeclareParameter<double>(node, ns + ".backward_length");
+  planner_param_.backward_detection_length =
+    getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
   planner_param_.ignore_width_from_center_line =
     getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
-  planner_param_.max_future_movement_time =
-    getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
-  planner_param_.threshold_yaw_diff =
-    getOrDeclareParameter<double>(node, ns + ".threshold_yaw_diff");
   planner_param_.adjacent_extend_width =
     getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
   planner_param_.opposite_adjacent_extend_width =
     getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
+  planner_param_.max_future_movement_time =
+    getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
+  planner_param_.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
+  planner_param_.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
+  planner_param_.ttc_ego_minimal_velocity =
+    getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
 }
 
 void BlindSpotModuleManager::launchNewModules(

planning/behavior_velocity_blind_spot_module/src/scene.cpp

@@ -67,6 +67,9 @@ void BlindSpotModule::initializeRTCStatus()
 BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
   PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
 {
+  if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) {
+    return OverPassJudge{"already over the pass judge line. no plan needed."};
+  }
   const auto & input_path = *path;
 
   /* set stop-line and stop-judgement-line for base_link */
@@ -98,6 +101,7 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
 
   const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose);
   if (is_over_pass_judge) {
+    is_over_pass_judge_line_ = true;
     return is_over_pass_judge.value();
   }
 
@@ -121,7 +125,8 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
   const auto & detection_area = detection_area_opt.value();
   debug_data_.detection_area = detection_area;
 
-  const auto ego_time_to_reach_stop_line = computeTimeToPassStopLine(blind_spot_lanelets);
+  const auto ego_time_to_reach_stop_line = computeTimeToPassStopLine(
+    blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose);
   /* calculate dynamic collision around detection area */
   const auto collision_obstacle = isCollisionDetected(
     blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose, detection_area,
@@ -439,14 +444,19 @@ std::optional<OverPassJudge> BlindSpotModule::isOverPassJudge(
 }
 
 double BlindSpotModule::computeTimeToPassStopLine(
-  const lanelet::ConstLanelets & blind_spot_lanelets) const
+  const lanelet::ConstLanelets & blind_spot_lanelets,
+  const geometry_msgs::msg::Pose & stop_line_pose) const
 {
+  // egoが停止している時にそのまま速度を使うと衝突しなくなってしまうのでegoについては最低速度を使う
   const auto & current_pose = planner_data_->current_odometry->pose;
   const auto current_arc_ego =
     lanelet::utils::getArcCoordinates(blind_spot_lanelets, current_pose).length;
-  const auto remaining_distance =
-    lanelet::utils::getLaneletLength3d(blind_spot_lanelets) - current_arc_ego;
-  return remaining_distance / (planner_data_->current_velocity->twist.linear.x);
+  const auto stopline_arc_ego =
+    lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length;
+  const auto remaining_distance = stopline_arc_ego - current_arc_ego;
+  return remaining_distance / std::max<double>(
+                                planner_param_.ttc_ego_minimal_velocity,
+                                planner_data_->current_velocity->twist.linear.x);
 }
 
 std::optional<autoware_auto_perception_msgs::msg::PredictedObject>
@@ -480,7 +490,8 @@ BlindSpotModule::isCollisionDetected(
       (object_arc_length - stop_line_arc_ego) /
       (object.kinematics.initial_twist_with_covariance.twist.linear.x);
     const auto ttc = ego_time_to_reach_stop_line - object_time_to_reach_stop_line;
-    if (-2.0 < ttc && ttc < 2.0) {
+    RCLCPP_INFO(logger_, "object ttc is %f", ttc);
+    if (planner_param_.ttc_min < ttc && ttc < planner_param_.ttc_max) {
       return object;
     }
   }
@@ -677,7 +688,7 @@ std::optional<lanelet::CompoundPolygon3d> BlindSpotModule::generateBlindSpotPoly
   const auto stop_line_arc_ego =
     lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length;
   const auto detection_area_start_length_ego =
-    std::max<double>(stop_line_arc_ego - planner_param_.backward_length, 0.0);
+    std::max<double>(stop_line_arc_ego - planner_param_.backward_detection_length, 0.0);
   return lanelet::utils::getPolygonFromArcLength(
     blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego);
 }

planning/behavior_velocity_blind_spot_module/src/scene.hpp

@@ -92,16 +92,16 @@ class BlindSpotModule : public SceneModuleInterface
 public:
   struct PlannerParam
   {
-    bool use_pass_judge_line;  //! distance which ego can stop with max brake
-    double stop_line_margin;   //! distance from auto-generated stopline to detection_area boundary
-    double backward_length;  //! distance[m] from closest path point to the edge of beginning point
-    double ignore_width_from_center_line;  //! ignore width from center line from detection_area
-    double
-      max_future_movement_time;  //! maximum time[second] for considering future movement of object
-    double threshold_yaw_diff;   //! threshold of yaw difference between ego and target object
-    double
-      adjacent_extend_width;  //! the width of extended detection/conflict area on adjacent lane
+    bool use_pass_judge_line;
+    double stop_line_margin;
+    double backward_detection_length;
+    double ignore_width_from_center_line;
+    double adjacent_extend_width;
     double opposite_adjacent_extend_width;
+    double max_future_movement_time;
+    double ttc_min;
+    double ttc_max;
+    double ttc_ego_minimal_velocity;
   };
 
   BlindSpotModule(
@@ -171,7 +171,9 @@ class BlindSpotModule : public SceneModuleInterface
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const geometry_msgs::msg::Pose & stop_point_pose) const;
 
-  double computeTimeToPassStopLine(const lanelet::ConstLanelets & blind_spot_lanelets) const;
+  double computeTimeToPassStopLine(
+    const lanelet::ConstLanelets & blind_spot_lanelets,
+    const geometry_msgs::msg::Pose & stop_line_pose) const;
 
   /**
    * @brief Check obstacle is in blind spot areas.