Feat/behavior velocity memorize last observed tl (#37)

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

Before this commit, is_permanent_go_ became TRUE if ego is
- before default stop line OR
- over (1st) pass judge line OR
- middle speed (larger than keep_detection_velocity_threshold)

AND

- over default stop line AND
- over (1st) pass judge line AND
- is_go_out_ AND
- previously SAFE

, which is equal to

- over default stop line AND
- over (1st) pass judge line AND
- middle speed AND
- is_go_out_ AND
- previously SAFE

.

With this commit, is_go_out_ condition is removed because it is equivalent to "previously SAFE" when auto-mode, and when manual-mode still it is equivaent to it for the following reason:
- even if occlusion/collision does not exist but the occlusion is disapproved, this module calculates "ext_occlusion_requested" and this state is judged as occluded. So the previous state is OCCLUDED, which means "previously SAFE" == false and it matches is_go_out_(this value is set to false by occlusion_activated_ through RTC)

In manual-mode RTC approval is used regardless of how this module judges, so when both "intersection" and "intersection_occlusion" is disapproved, ego will keep stopping at the default_stopline(if disapproval is not too late and did not pass the sole pass judge line) although the internal state changes from "FirstWaitBefoPeeking" to "PeekingTowardOcclusion". Then if "intersection" is approved, still the state is OCCLUDED(because ext_occlusion_requested) and "previously SAFE" == false matches is_go_out_ == false. If only "intersection_occlusion" is approved and "intersection" is disapproved, it is expected that ego is NOT after default_stopline(maybe this is not the case due to low control precision) and NOT after pass_judge_line(it is highly expected because if ego is stopped at the default stopline pass judge line is almost at the first_attention_area_stopline) so it will keep stopping.

The time when ego passed 1st/2nd pass judge line safely is stored, and the visualization of each pass judge line pose is red if ego passed the pass judge line safely.

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

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -7,6 +7,7 @@
         attention_area_angle_threshold: 0.785
         use_intersection_area: false
         default_stopline_margin: 3.0
+        second_pass_judge_line_margin: 1.0
         stopline_overshoot_margin: 0.5
         path_interpolation_ds: 0.1
         max_accel: -2.8
@@ -36,7 +37,6 @@
         consider_wrong_direction_vehicle: false
         collision_detection_hold_time: 0.5
         min_predicted_path_confidence: 0.05
-        keep_detection_velocity_threshold: 0.833
         velocity_profile:
           use_upstream: true
           minimum_upstream_velocity: 0.01

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -90,7 +90,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray(
   marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
   marker_line.action = visualization_msgs::msg::Marker::ADD;
   marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
-  marker_line.scale = createMarkerScale(0.1, 0.0, 0.0);
+  marker_line.scale = createMarkerScale(0.2, 0.0, 0.0);
   marker_line.color = createMarkerColor(r, g, b, 0.999);
 
   const double yaw = tf2::getYaw(pose.orientation);
@@ -283,11 +283,23 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
     &debug_marker_array, now);
   */
 
-  if (debug_data_.pass_judge_wall_pose) {
+  if (debug_data_.first_pass_judge_wall_pose) {
+    const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0;
+    const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0;
     appendMarkerArray(
       createPoseMarkerArray(
-        debug_data_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85,
-        0.9),
+        debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r,
+        g, 0.0),
+      &debug_marker_array, now);
+  }
+
+  if (debug_data_.second_pass_judge_wall_pose) {
+    const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0;
+    const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0;
+    appendMarkerArray(
+      createPoseMarkerArray(
+        debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_,
+        r, g, 0.0),
       &debug_marker_array, now);
   }
 

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -53,6 +53,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".common.use_intersection_area");
   ip.common.default_stopline_margin =
     getOrDeclareParameter<double>(node, ns + ".common.default_stopline_margin");
+  ip.common.second_pass_judge_line_margin =
+    getOrDeclareParameter<double>(node, ns + ".common.second_pass_judge_line_margin");
   ip.common.stopline_overshoot_margin =
     getOrDeclareParameter<double>(node, ns + ".common.stopline_overshoot_margin");
   ip.common.path_interpolation_ds =
@@ -92,8 +94,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".collision_detection.collision_detection_hold_time");
   ip.collision_detection.min_predicted_path_confidence =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.min_predicted_path_confidence");
-  ip.collision_detection.keep_detection_velocity_threshold = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.keep_detection_velocity_threshold");
   ip.collision_detection.velocity_profile.use_upstream =
     getOrDeclareParameter<bool>(node, ns + ".collision_detection.velocity_profile.use_upstream");
   ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter<double>(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1041,7 +1041,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto default_stopline_idx_opt = intersection_stoplines.default_stopline;
   const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline;
   const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline;
-  const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line;
+  const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line;
+  const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line;
   const auto occlusion_wo_tl_pass_judge_line_idx =
     intersection_stoplines.occlusion_wo_tl_pass_judge_line;
 
@@ -1221,44 +1222,72 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     prev_occlusion_status_ = occlusion_status;
   }
 
-  // TODO(Mamoru Sobue): this part needs more formal handling
-  const size_t pass_judge_line_idx = [=]() {
+  const size_t pass_judge_line_idx = [&]() {
     if (enable_occlusion_detection_) {
-      // if occlusion detection is enabled, pass_judge position is beyond the boundary of first
-      // attention area
       if (has_traffic_light_) {
-        return occlusion_stopline_idx;
+        // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its
+        // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line
+        if (passed_1st_judge_line_while_peeking_) {
+          return occlusion_stopline_idx;
+        }
+        const bool is_over_first_pass_judge_line =
+          util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx);
+        if (is_occlusion_state && is_over_first_pass_judge_line) {
+          passed_1st_judge_line_while_peeking_ = true;
+          return occlusion_stopline_idx;
+        } else {
+          return first_pass_judge_line_idx;
+        }
       } else if (is_occlusion_state) {
         // if there is no traffic light and occlusion is detected, pass_judge position is beyond
         // the boundary of first attention area
         return occlusion_wo_tl_pass_judge_line_idx;
       } else {
         // if there is no traffic light and occlusion is not detected, pass_judge position is
         // default
-        return default_pass_judge_line_idx;
+        return first_pass_judge_line_idx;
       }
     }
-    return default_pass_judge_line_idx;
+    return first_pass_judge_line_idx;
   }();
-  debug_data_.pass_judge_wall_pose =
-    planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
-  const bool is_over_pass_judge_line =
-    util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
-  const double vel_norm = std::hypot(
-    planner_data_->current_velocity->twist.linear.x,
-    planner_data_->current_velocity->twist.linear.y);
-  const bool keep_detection =
-    (vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold);
+
   const bool was_safe = std::holds_alternative<IntersectionModule::Safe>(prev_decision_result_);
-  // if ego is over the pass judge line and not stopped
-  if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) {
-    RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection");
-    // do nothing
-  } else if (
-    (was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) ||
-    is_permanent_go_) {
-    // is_go_out_: previous RTC approval
-    // activated_: current RTC approval
+
+  const bool is_over_1st_pass_judge_line =
+    util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
+  if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) {
+    safely_passed_1st_judge_line_time_ = clock_->now();
+  }
+  debug_data_.first_pass_judge_wall_pose =
+    planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
+  debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value();
+  const bool is_over_2nd_pass_judge_line =
+    util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx);
+  if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) {
+    safely_passed_2nd_judge_line_time_ = clock_->now();
+  }
+  debug_data_.second_pass_judge_wall_pose =
+    planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path);
+  debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value();
+
+  if ((is_over_default_stopline && is_over_2nd_pass_judge_line && was_safe) || is_permanent_go_) {
+    /*
+     * This body is active if ego is
+     * - over the default stopline AND
+     * - over the 1st && 2nd pass judge line AND
+     * - previously safe
+     * ,
+     * which means ego can stop even if it is over the 1st pass judge line but
+     * - before default stopline OR
+     * - before the 2nd pass judge line OR
+     * - or previously unsafe
+     * .
+     * In order for ego to continue peeking or collision detection when occlusion is detected after
+     * ego passed the 1st pass judge line, it needs to be
+     * - before the default stopline OR
+     * - before the 2nd pass judge line OR
+     * - previously unsafe
+     */
     is_permanent_go_ = true;
     return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"};
   }
@@ -1309,6 +1338,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       : (planner_param_.collision_detection.collision_detection_hold_time -
          collision_state_machine_.getDuration());
 
+  // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd
+  // pass judge line respectively, ego should go
   const bool has_collision = checkCollision(
     *path, &target_objects, path_lanelets, closest_idx,
     std::min<size_t>(occlusion_stopline_idx, path->points.size() - 1), time_to_restart,
@@ -1677,6 +1708,7 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
   const double max_jerk = planner_param_.common.max_jerk;
   const double delay_response_time = planner_param_.common.delay_response_time;
   const double peeking_offset = planner_param_.occlusion.peeking_offset;
+  const double second_pass_judge_line_margin = planner_param_.common.second_pass_judge_line_margin;
 
   const auto first_attention_area = first_attention_lane.polygon3d();
   const auto first_attention_lane_centerline = first_attention_lane.centerline2d();
@@ -1814,10 +1846,13 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
     second_attention_stopline_ip_int >= 0 ? static_cast<size_t>(second_attention_stopline_ip_int)
                                           : 0;
 
-  // (8) second pass judge lie position on interpolated path
-  int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds);
+  // (8) second pass judge line position on interpolated path. It is the same as first pass judge
+  // line if second_attention_lane is null
+  int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds) -
+                                 std::ceil(second_pass_judge_line_margin / ds);
   const auto second_pass_judge_line_ip =
-    static_cast<size_t>(std::max<int>(second_pass_judge_ip_int, 0));
+    second_attention_area_opt ? static_cast<size_t>(std::max<int>(second_pass_judge_ip_int, 0))
+                              : first_pass_judge_line_ip;
 
   struct IntersectionStopLinesTemp
   {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -48,7 +48,10 @@ struct DebugData
   std::optional<geometry_msgs::msg::Pose> collision_stop_wall_pose{std::nullopt};
   std::optional<geometry_msgs::msg::Pose> occlusion_stop_wall_pose{std::nullopt};
   std::optional<geometry_msgs::msg::Pose> occlusion_first_stop_wall_pose{std::nullopt};
-  std::optional<geometry_msgs::msg::Pose> pass_judge_wall_pose{std::nullopt};
+  std::optional<geometry_msgs::msg::Pose> first_pass_judge_wall_pose{std::nullopt};
+  bool passed_first_pass_judge{false};
+  bool passed_second_pass_judge{false};
+  std::optional<geometry_msgs::msg::Pose> second_pass_judge_wall_pose{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> attention_area{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
@@ -256,8 +259,8 @@ struct IntersectionStopLines
   size_t first_pass_judge_line{0};
 
   /**
-   * second_pass_judge_line is before second_attention_stopline by the braking distance. if its
-   * value is calculated negative, it is 0
+   * second_pass_judge_line is before second_attention_stopline by the braking distance. if
+   * second_attention_lane is null, it is same as first_pass_judge_line
    */
   size_t second_pass_judge_line{0};
 
@@ -339,6 +342,7 @@ class IntersectionModule : public SceneModuleInterface
       double attention_area_angle_threshold;
       bool use_intersection_area;
       double default_stopline_margin;
+      double second_pass_judge_line_margin;
       double stopline_overshoot_margin;
       double path_interpolation_ds;
       double max_accel;
@@ -373,7 +377,6 @@ class IntersectionModule : public SceneModuleInterface
       bool consider_wrong_direction_vehicle;
       double collision_detection_hold_time;
       double min_predicted_path_confidence;
-      double keep_detection_velocity_threshold;
       struct VelocityProfile
       {
         bool use_upstream;
@@ -606,7 +609,7 @@ class IntersectionModule : public SceneModuleInterface
 
 private:
   rclcpp::Node & node_;
-  const int64_t lane_id_;
+  const lanelet::Id lane_id_;
   const std::set<lanelet::Id> associative_ids_;
   const std::string turn_direction_;
   const bool has_traffic_light_;
@@ -621,6 +624,9 @@ class IntersectionModule : public SceneModuleInterface
   bool is_permanent_go_{false};
   DecisionResult prev_decision_result_{Indecisive{""}};
   OcclusionType prev_occlusion_status_;
+  bool passed_1st_judge_line_while_peeking_{false};
+  std::optional<rclcpp::Time> safely_passed_1st_judge_line_time_{std::nullopt};
+  std::optional<rclcpp::Time> safely_passed_2nd_judge_line_time_{std::nullopt};
 
   // for occlusion detection
   const bool enable_occlusion_detection_;