feat(dummy_infrastructure): auto approval when ego stops at stop line (#10223)

feat(dummy_infrastructur): auto approval when ego stops at stop line

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

system/autoware_dummy_infrastructure/config/dummy_infrastructure.param.yaml

@@ -1,8 +1,11 @@
 /**:
   ros__parameters:
     update_rate_hz: 10.0
-    use_first_command: true
-    use_command_state: false
-    instrument_id: ""
-    approval: false
-    is_finalized: false
+    use_first_command: false
+    use_command_state: true
+    instrument_id: '0'
+    approval: true
+    is_finalized: true
+    auto_approval_mode: true
+    stop_distance_threshold: 1.0
+    stop_velocity_threshold: 0.1

system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp

@@ -15,18 +15,27 @@
 #ifndef AUTOWARE__DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_
 #define AUTOWARE__DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_
 
+#include "autoware_utils/ros/polling_subscriber.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
 #include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
 #include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
 
 #include <chrono>
 #include <memory>
+#include <optional>
+#include <set>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::dummy_infrastructure
 {
+using nav_msgs::msg::Odometry;
+using tier4_planning_msgs::msg::PlanningFactorArray;
 using tier4_v2x_msgs::msg::InfrastructureCommand;
 using tier4_v2x_msgs::msg::InfrastructureCommandArray;
 using tier4_v2x_msgs::msg::VirtualTrafficLightState;
@@ -45,17 +54,23 @@ class DummyInfrastructureNode : public rclcpp::Node
     std::string instrument_id{};
     bool approval{};
     bool is_finalized{};
+    bool auto_approval_mode{};
+    double stop_distance_threshold{};
+    double stop_velocity_threshold{};
   };
 
 private:
-  // Subscriber
-  rclcpp::Subscription<InfrastructureCommandArray>::SharedPtr sub_command_array_{};
-
-  // Callback
-  void onCommandArray(const InfrastructureCommandArray::ConstSharedPtr msg);
+  autoware_utils::InterProcessPollingSubscriber<InfrastructureCommandArray> sub_command_array_{
+    this, "~/input/command_array"};
+  autoware_utils::InterProcessPollingSubscriber<PlanningFactorArray> sub_planning_factors_{
+    this, "~/input/planning_factors"};
+  autoware_utils::InterProcessPollingSubscriber<Odometry> sub_odometry_{this, "~/input/odometry"};
 
   // Data Buffer
   InfrastructureCommandArray::ConstSharedPtr command_array_{};
+  PlanningFactorArray::ConstSharedPtr planning_factors_{};
+  Odometry::ConstSharedPtr current_odometry_{};
+  std::set<std::string> approved_command_ids_{};
 
   // Publisher
   rclcpp::Publisher<VirtualTrafficLightStateArray>::SharedPtr pub_state_array_{};
@@ -66,6 +81,12 @@ class DummyInfrastructureNode : public rclcpp::Node
   bool isDataReady();
   void onTimer();
 
+  // Auto approval check
+  std::pair<bool, bool> getApprovalConditions() const;
+  std::optional<std::string> getCurrentCommandId() const;
+  std::pair<bool, bool> checkApprovalCommand(
+    const std::string & command_id, const bool is_stopped, const bool is_near_stop_line) const;
+
   // Parameter Server
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
   rcl_interfaces::msg::SetParametersResult onSetParam(

system/autoware_dummy_infrastructure/launch/dummy_infrastructure.launch.xml

@@ -2,6 +2,8 @@
 <launch>
   <!-- Input -->
   <arg name="input/command_array" default="/planning/scenario_planning/status/infrastructure_commands"/>
+  <arg name="input/planning_factors" default="/planning/planning_factors/virtual_traffic_light"/>
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
 
   <!-- Output -->
   <arg name="output/state_array" default="/system/v2x/virtual_traffic_light_states"/>
@@ -13,6 +15,8 @@
   <node pkg="autoware_dummy_infrastructure" exec="autoware_dummy_infrastructure_node" name="dummy_infrastructure" output="screen">
     <!-- Input -->
     <remap from="~/input/command_array" to="$(var input/command_array)"/>
+    <remap from="~/input/planning_factors" to="$(var input/planning_factors)"/>
+    <remap from="~/input/odometry" to="$(var input/odometry)"/>
 
     <!-- Output -->
     <remap from="~/output/state_array" to="$(var output/state_array)"/>

system/autoware_dummy_infrastructure/package.xml

@@ -17,8 +17,10 @@
 
   <build_depend>libboost-dev</build_depend>
 
+  <depend>autoware_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_planning_msgs</depend>
   <depend>tier4_v2x_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>

system/autoware_dummy_infrastructure/schema/dummy_infrastructure.schema.json

@@ -29,12 +29,27 @@
         "approval": {
           "type": "boolean",
           "default": "false",
-          "description": "set approval filed to ros param"
+          "description": "Set approval field to ros param"
         },
         "is_finalized": {
           "type": "boolean",
           "default": "false",
           "description": "Stop at stop_line if finalization isn't completed"
+        },
+        "auto_approval_mode": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Automatically approve commands when vehicle has stopped near a virtual traffic light"
+        },
+        "stop_distance_threshold": {
+          "type": "number",
+          "default": "1.0",
+          "description": "Distance threshold to determine if the vehicle is near a stop line [m]"
+        },
+        "stop_velocity_threshold": {
+          "type": "number",
+          "default": "0.1",
+          "description": "Velocity threshold to determine if the vehicle has stopped [m/s]"
         }
       },
       "required": [
@@ -43,7 +58,10 @@
         "use_command_state",
         "instrument_id",
         "approval",
-        "is_finalized"
+        "is_finalized",
+        "auto_approval_mode",
+        "stop_distance_threshold",
+        "stop_velocity_threshold"
       ],
       "additionalProperties": false
     }

system/autoware_dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp

@@ -16,8 +16,13 @@
 
 #include <boost/optional.hpp>
 
+#include <algorithm>
+#include <cmath>
+#include <limits>
 #include <memory>
+#include <optional>
 #include <string>
+#include <utility>
 #include <vector>
 
 using namespace std::literals;
@@ -46,7 +51,7 @@ bool update_param(
   return true;
 }
 
-boost::optional<InfrastructureCommandArray> findCommand(
+std::optional<InfrastructureCommandArray> findCommand(
   const InfrastructureCommandArray & command_array, const std::string & instrument_id,
   const bool use_first_command, const bool use_command_state)
 {
@@ -72,12 +77,44 @@ boost::optional<InfrastructureCommandArray> findCommand(
       found_flag = true;
     }
   }
-  if (found_flag) {
-    return array;
-  } else {
-    return {};
+
+  return found_flag ? std::optional<InfrastructureCommandArray>{array} : std::nullopt;
+}
+
+std::optional<double> calcClosestStopLineDistance(
+  const PlanningFactorArray::ConstSharedPtr & planning_factors)
+{
+  if (!planning_factors || planning_factors->factors.empty()) {
+    return std::nullopt;
+  }
+
+  const auto vtl_it = std::find_if(
+    planning_factors->factors.begin(), planning_factors->factors.end(),
+    [](const auto & factor) { return factor.module == "virtual_traffic_light"; });
+
+  if (vtl_it == planning_factors->factors.end()) {
+    return std::nullopt;
+  }
+
+  std::vector<double> distances;
+  distances.reserve(planning_factors->factors.size() * 2);
+
+  for (const auto & factor : planning_factors->factors) {
+    if (factor.module == "virtual_traffic_light") {
+      for (const auto & control_point : factor.control_points) {
+        distances.push_back(std::abs(control_point.distance));
+      }
+    }
+  }
+
+  if (distances.empty()) {
+    return std::nullopt;
   }
+
+  const auto min_it = std::min_element(distances.begin(), distances.end());
+  return *min_it;
 }
+
 }  // namespace
 
 DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & node_options)
@@ -95,11 +132,9 @@ DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & nod
   node_param_.instrument_id = declare_parameter<std::string>("instrument_id");
   node_param_.approval = declare_parameter<bool>("approval");
   node_param_.is_finalized = declare_parameter<bool>("is_finalized");
-
-  // Subscriber
-  sub_command_array_ = create_subscription<InfrastructureCommandArray>(
-    "~/input/command_array", rclcpp::QoS{1},
-    std::bind(&DummyInfrastructureNode::onCommandArray, this, _1));
+  node_param_.auto_approval_mode = declare_parameter<bool>("auto_approval_mode", false);
+  node_param_.stop_distance_threshold = declare_parameter<double>("stop_distance_threshold", 1.0);
+  node_param_.stop_velocity_threshold = declare_parameter<double>("stop_velocity_threshold", 0.1);
 
   // Publisher
   pub_state_array_ = create_publisher<VirtualTrafficLightStateArray>("~/output/state_array", 1);
@@ -110,17 +145,12 @@ DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & nod
     this, get_clock(), update_period_ns, std::bind(&DummyInfrastructureNode::onTimer, this));
 }
 
-void DummyInfrastructureNode::onCommandArray(const InfrastructureCommandArray::ConstSharedPtr msg)
-{
-  if (!msg->commands.empty()) {
-    command_array_ = msg;
-  }
-}
-
 rcl_interfaces::msg::SetParametersResult DummyInfrastructureNode::onSetParam(
   const std::vector<rclcpp::Parameter> & params)
 {
   rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
 
   try {
     // Copy to local variable
@@ -133,17 +163,23 @@ rcl_interfaces::msg::SetParametersResult DummyInfrastructureNode::onSetParam(
     update_param(params, "instrument_id", p.instrument_id);
     update_param(params, "approval", p.approval);
     update_param(params, "is_finalized", p.is_finalized);
+    update_param(params, "auto_approval_mode", p.auto_approval_mode);
+    update_param(params, "stop_distance_threshold", p.stop_distance_threshold);
+    update_param(params, "stop_velocity_threshold", p.stop_velocity_threshold);
 
     // Copy back to member variable
     node_param_ = p;
   } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
     result.successful = false;
     result.reason = e.what();
-    return result;
+  } catch (const std::exception & e) {
+    result.successful = false;
+    result.reason = "Exception occurred: " + std::string(e.what());
+  } catch (...) {
+    result.successful = false;
+    result.reason = "Unknown exception occurred";
   }
 
-  result.successful = true;
-  result.reason = "success";
   return result;
 }
 
@@ -154,11 +190,33 @@ bool DummyInfrastructureNode::isDataReady()
     return false;
   }
 
+  if (!planning_factors_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for planning_factors msg...");
+    return false;
+  }
+
+  if (!current_odometry_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for odometry msg...");
+    return false;
+  }
+
   return true;
 }
 
+std::optional<std::string> DummyInfrastructureNode::getCurrentCommandId() const
+{
+  if (!command_array_ || command_array_->commands.empty()) {
+    return std::nullopt;
+  }
+  return command_array_->commands.front().id;
+}
+
 void DummyInfrastructureNode::onTimer()
 {
+  command_array_ = sub_command_array_.take_data();
+  planning_factors_ = sub_planning_factors_.take_data();
+  current_odometry_ = sub_odometry_.take_data();
+
   if (!isDataReady()) {
     return;
   }
@@ -174,24 +232,59 @@ void DummyInfrastructureNode::onTimer()
     state.stamp = get_clock()->now();
     state.id = node_param_.instrument_id;
     state.type = "dummy_infrastructure";
-    state.approval = false;
+    state.approval = node_param_.approval;
     state.is_finalized = node_param_.is_finalized;
     state_array.states.push_back(state);
   } else {
-    for (auto command : found_command_array->commands) {
-      VirtualTrafficLightState state;
+    const bool is_stopped =
+      std::abs(current_odometry_->twist.twist.linear.x) < node_param_.stop_velocity_threshold;
+    const auto min_distance_opt = calcClosestStopLineDistance(planning_factors_);
+    const bool is_near_stop_line =
+      min_distance_opt && *min_distance_opt <= node_param_.stop_distance_threshold;
+
+    for (const auto & command : found_command_array->commands) {
+      const auto [command_approval, command_is_finalized] =
+        checkApprovalCommand(command.id, is_stopped, is_near_stop_line);
+      if (command_approval && command_is_finalized) {
+        if (approved_command_ids_.find(command.id) == approved_command_ids_.end()) {
+          approved_command_ids_.insert(command.id);
+          RCLCPP_INFO(get_logger(), "Approved new command ID %s", command.id.c_str());
+        }
+      }
+      VirtualTrafficLightState state{};
       state.stamp = get_clock()->now();
       state.id = command.id;
       state.type = command.type;
-      state.approval = node_param_.approval;
-      state.is_finalized = node_param_.is_finalized;
+      state.approval = command_approval;
+      state.is_finalized = command_is_finalized;
       state_array.states.push_back(state);
     }
   }
 
   pub_state_array_->publish(state_array);
 }
 
+std::pair<bool, bool> DummyInfrastructureNode::checkApprovalCommand(
+  const std::string & command_id, const bool is_stopped, const bool is_near_stop_line) const
+{
+  if (!node_param_.auto_approval_mode) {
+    return {node_param_.approval, node_param_.is_finalized};
+  }
+
+  if (approved_command_ids_.find(command_id) != approved_command_ids_.end()) {
+    return {true, true};
+  }
+
+  if (is_stopped && is_near_stop_line) {
+    if (approved_command_ids_.find(command_id) == approved_command_ids_.end()) {
+      RCLCPP_INFO(get_logger(), "Command ID %s meets approval conditions", command_id.c_str());
+    }
+    return {true, true};
+  }
+
+  return {false, false};
+}
+
 }  // namespace autoware::dummy_infrastructure
 
 #include <rclcpp_components/register_node_macro.hpp>