chore(blind_spot): PR7110 and PR7164

planning/behavior_velocity_blind_spot_module/CMakeLists.txt

@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/debug.cpp
   src/manager.cpp
   src/scene.cpp
+  src/decisions.cpp
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)

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/debug.cpp

@@ -71,55 +71,16 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray(
   return msg;
 }
 
-visualization_msgs::msg::MarkerArray createPoseMarkerArray(
-  const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns,
-  const int64_t id, const double r, const double g, const double b)
-{
-  visualization_msgs::msg::MarkerArray msg;
-
-  if (state == StateMachine::State::STOP) {
-    visualization_msgs::msg::Marker marker_line{};
-    marker_line.header.frame_id = "map";
-    marker_line.ns = ns + "_line";
-    marker_line.id = id;
-    marker_line.lifetime = rclcpp::Duration::from_seconds(0.3);
-    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.color = createMarkerColor(r, g, b, 0.999);
-
-    const double yaw = tf2::getYaw(pose.orientation);
-
-    const double a = 3.0;
-    geometry_msgs::msg::Point p0;
-    p0.x = pose.position.x - a * std::sin(yaw);
-    p0.y = pose.position.y + a * std::cos(yaw);
-    p0.z = pose.position.z;
-    marker_line.points.push_back(p0);
-
-    geometry_msgs::msg::Point p1;
-    p1.x = pose.position.x + a * std::sin(yaw);
-    p1.y = pose.position.y - a * std::cos(yaw);
-    p1.z = pose.position.z;
-    marker_line.points.push_back(p1);
-
-    msg.markers.push_back(marker_line);
-  }
-
-  return msg;
-}
-
 }  // namespace
 
 motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls()
 {
   motion_utils::VirtualWalls virtual_walls;
 
-  if (!isActivated() && !is_over_pass_judge_line_) {
+  if (debug_data_.virtual_wall_pose) {
     motion_utils::VirtualWall wall;
     wall.text = "blind_spot";
-    wall.pose = debug_data_.virtual_wall_pose;
+    wall.pose = debug_data_.virtual_wall_pose.value();
     wall.ns = std::to_string(module_id_) + "_";
     virtual_walls.push_back(wall);
   }
@@ -130,28 +91,14 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
 {
   visualization_msgs::msg::MarkerArray debug_marker_array;
 
-  const auto state = state_machine_.getState();
   const auto now = this->clock_->now();
 
-  appendMarkerArray(
-    createPoseMarkerArray(
-      debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0),
-    &debug_marker_array, now);
-
-  appendMarkerArray(
-    createPoseMarkerArray(
-      debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5),
-    &debug_marker_array, now);
-
-  appendMarkerArray(
-    createLaneletPolygonsMarkerArray(
-      debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5),
-    &debug_marker_array, now);
-
-  appendMarkerArray(
-    createLaneletPolygonsMarkerArray(
-      debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0),
-    &debug_marker_array, now);
+  if (debug_data_.detection_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0),
+      &debug_marker_array, now);
+  }
 
   appendMarkerArray(
     debug::createObjectsMarkerArray(

planning/behavior_velocity_blind_spot_module/src/decisions.cpp

@@ -0,0 +1,154 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "scene.hpp"
+
+#include <motion_utils/trajectory/trajectory.hpp>
+
+namespace behavior_velocity_planner
+{
+
+/*
+ * for default
+ */
+template <typename T>
+void BlindSpotModule::setRTCStatusByDecision(
+  const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  static_assert("Unsupported type passed to setRTCStatus");
+  return;
+}
+
+template <typename T>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  [[maybe_unused]] const T & decision,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason)
+{
+  static_assert("Unsupported type passed to reactRTCApprovalByDecision");
+}
+
+/*
+ * for InternalError
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  [[maybe_unused]] const InternalError & decision,
+  [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  [[maybe_unused]] const InternalError & decision,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason)
+{
+  return;
+}
+
+/*
+ * For OverPassJudge
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  [[maybe_unused]] const OverPassJudge & decision,
+  [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  [[maybe_unused]] const OverPassJudge & decision,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason)
+{
+  return;
+}
+
+/*
+ * for Unsafe
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  setSafe(false);
+  const auto & current_pose = planner_data_->current_odometry->pose;
+  setDistance(
+    motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  StopReason * stop_reason)
+{
+  if (!isActivated()) {
+    constexpr double stop_vel = 0.0;
+    planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
+    debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
+      decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
+
+    tier4_planning_msgs::msg::StopFactor stop_factor;
+    const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
+    stop_factor.stop_pose = stop_pose;
+    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
+    planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
+  }
+  return;
+}
+
+/*
+ * for Safe
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  setSafe(true);
+  const auto & current_pose = planner_data_->current_odometry->pose;
+  setDistance(
+    motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  StopReason * stop_reason)
+{
+  if (!isActivated()) {
+    constexpr double stop_vel = 0.0;
+    planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
+    debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
+      decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
+
+    tier4_planning_msgs::msg::StopFactor stop_factor;
+    const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
+    stop_factor.stop_pose = stop_pose;
+    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
+    planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
+  }
+  return;
+}
+
+}  // namespace behavior_velocity_planner

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(
@@ -63,14 +66,17 @@ void BlindSpotModuleManager::launchNewModules(
     }
 
     // Is turning lane?
-    const std::string turn_direction = ll.attributeOr("turn_direction", "else");
-    if (turn_direction != "left" && turn_direction != "right") {
+    const std::string turn_direction_str = ll.attributeOr("turn_direction", "else");
+    if (turn_direction_str != "left" && turn_direction_str != "right") {
       continue;
     }
+    const auto turn_direction = turn_direction_str == "left"
+                                  ? BlindSpotModule::TurnDirection::LEFT
+                                  : BlindSpotModule::TurnDirection::RIGHT;
 
     registerModule(std::make_shared<BlindSpotModule>(
-      module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"),
-      clock_));
+      module_id, lane_id, turn_direction, planner_data_, planner_param_,
+      logger_.get_child("blind_spot_module"), clock_));
     generateUUID(module_id);
     updateRTCStatus(
       getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);