fix(vtl_adapter): fix operation mode subscription

.pre-commit-config.yaml

@@ -1,7 +1,8 @@
 repos:
   - repo: https://github.com/tier4/pre-commit-hooks-ros
-    rev: v0.8.0
+    rev: v0.10.1
     hooks:
       - id: flake8-ros
+        additional_dependencies: [flake8,setuptools==65.0.0]
 
 exclude: .svg

v2i_interface/scripts/test/v2i_interface_test.py

@@ -212,7 +212,6 @@ def main_loop(self):
                 self._fp_send.write(', '.join('{}'.format(x)
                                               for x in reply_array))
                 self._fp_send.write('\n')
-            window.refresh()
             next_sleep = (
                 (((self._base_time - time.time()) * 1000) %
                  self._window_refresh_interval_ms) or self._window_refresh_interval_ms) / 1000
@@ -233,7 +232,6 @@ def recv_from_v2i_interface(self):
             pass
 
     def run(self):
-        global window
         global is_send_write
         global is_recv_write
         os.makedirs(os.path.dirname(self._recv_output_filename), exist_ok=True)

vtl_adapter/include/vtl_adapter/eve_vtl_interface_converter.hpp

@@ -36,7 +36,8 @@ class EveVTLInterfaceConverter
 {
 public:
   EveVTLInterfaceConverter(
-    const InfrastructureCommand& input_command, rclcpp::Node* node);
+    const InfrastructureCommand& input_command, rclcpp::Node* node,
+    const OperationModeState::ConstSharedPtr& operation_mode_state);
 
   const std::shared_ptr<EveVTLAttr>& vtlAttribute() const;
   const InfrastructureCommand& command() const;
@@ -48,17 +49,11 @@ class EveVTLInterfaceConverter
   std::string convertInfraCommand(const uint8_t& input_command) const;
   std::optional<std::string> convertADState() const;
 
-  // Subscription
-  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_state_;
-
-  // Callback
-  void onOperationModeState(const OperationModeState::ConstSharedPtr msg);
-
   // Member variables
   InfrastructureCommand command_;
   std::shared_ptr<EveVTLAttr> vtl_attr_;
   rclcpp::Node* node_;
-  uint8_t mode_;
+  OperationModeState::ConstSharedPtr operation_mode_state_ptr_;
 };
 
 }  // namespace eve_vtl_interface_converter

vtl_adapter/include/vtl_adapter/vtl_command_converter.hpp

@@ -23,6 +23,7 @@
 #include "v2i_interface_msgs/msg/infrastructure_command_array.hpp"
 
 // sub input
+#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
 
 #include "vtl_adapter/interface_converter_data_pipeline.hpp"
 #include "vtl_adapter/eve_vtl_interface_converter.hpp"
@@ -42,6 +43,7 @@ using InterfaceConverterMap =
   std::unordered_map<uint8_t, std::shared_ptr<InterfaceConverter>>;
 using IFConverterDataPipeline =
   interface_converter_data_pipeline::IFConverterDataPipeline;
+using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
 
 class VtlCommandConverter
 {
@@ -51,12 +53,18 @@ class VtlCommandConverter
   std::shared_ptr<IFConverterDataPipeline> converterPipeline();
   void onCommand(const MainInputCommandArr::ConstSharedPtr msg);
 private:
+  void onOperationModeState(const OperationModeState::ConstSharedPtr msg);
+
   // Node
   rclcpp::Node* node_;
 
   // Publisher
   rclcpp::Publisher<MainOutputCommandArr>::SharedPtr command_pub_;
 
+  // Subscription: /api/operation_mode/state (single subscription, shared by all converters)
+  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_state_;
+  OperationModeState::ConstSharedPtr latest_operation_mode_state_;
+
   // Preprocess
   std::shared_ptr<InterfaceConverterMultiMap> createConverter(
     const MainInputCommandArr::ConstSharedPtr& original_command) const;

vtl_adapter/src/eve_vtl_interface_converter.cpp

@@ -29,15 +29,11 @@ Class public function
 */
 
 EveVTLInterfaceConverter::EveVTLInterfaceConverter(
-  const InfrastructureCommand& input_command, rclcpp::Node* node)
-  : command_(input_command), node_(node)
+  const InfrastructureCommand& input_command, rclcpp::Node* node,
+  const OperationModeState::ConstSharedPtr& operation_mode_state)
+  : command_(input_command), node_(node), operation_mode_state_ptr_(operation_mode_state)
 {
-  using namespace std::placeholders;
   init(input_command);
-
-  sub_operation_mode_state_ = node->create_subscription<OperationModeState>(
-    "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(),
-    std::bind(&EveVTLInterfaceConverter::onOperationModeState, this, _1));
 }
 
 const std::shared_ptr<EveVTLAttr>& EveVTLInterfaceConverter::vtlAttribute() const
@@ -50,11 +46,6 @@ const InfrastructureCommand& EveVTLInterfaceConverter::command() const
   return command_;
 }
 
-void EveVTLInterfaceConverter::onOperationModeState(const OperationModeState::ConstSharedPtr msg)
-{
-  mode_ = msg->mode;
-}
-
 std::optional<uint8_t> EveVTLInterfaceConverter::request() const
 {
   if (!vtl_attr_) {
@@ -279,15 +270,21 @@ std::optional<std::string> EveVTLInterfaceConverter::convertADState() const
   bool is_valid_state = false;
 
   if (permit_state == eve_vtl_spec::VALUE_PERMIT_STATE_DRIVING) {
-    is_valid_state = (mode_ == OperationModeState::AUTONOMOUS);
+    if (operation_mode_state_ptr_ && operation_mode_state_ptr_->mode == OperationModeState::AUTONOMOUS) {
+      is_valid_state = true;
+    } else {
+      is_valid_state = false;
+    }
   } else if (permit_state == eve_vtl_spec::VALUE_PERMIT_STATE_NULL) {
     is_valid_state = true;
   }
 
   if (!is_valid_state) {
+    const int mode = operation_mode_state_ptr_ ? static_cast<int>(operation_mode_state_ptr_->mode) : -1;
     RCLCPP_WARN_STREAM_THROTTLE(
       node_->get_logger(), *node_->get_clock(), ERROR_THROTTLE_MSEC,
-      "EveVTLInterfaceConverter::" << __func__ << ": state is invalid: mode=" << mode_);
+      "EveVTLInterfaceConverter::" << __func__ << ": state is invalid: mode=" << mode
+        << ", permit_state=\"" << permit_state << "\"");
   }
   return (is_valid_state) ? permit_state_opt : std::nullopt;
 }

vtl_adapter/src/vtl_command_converter.cpp

@@ -36,6 +36,11 @@ void VtlCommandConverter::init(rclcpp::Node* node)
   auto subscriber_option = rclcpp::SubscriptionOptions();
   subscriber_option.callback_group = group;
 
+  // Subscription: /api/operation_mode/state (latest value shared by all converters)
+  sub_operation_mode_state_ = node->create_subscription<OperationModeState>(
+    "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(),
+    std::bind(&VtlCommandConverter::onOperationModeState, this, _1),
+    subscriber_option);
 
   // Publisher
   command_pub_ = node->create_publisher<MainOutputCommandArr>(
@@ -46,6 +51,11 @@ void VtlCommandConverter::init(rclcpp::Node* node)
     "VtlCommandConverter: initialized.");
 }
 
+void VtlCommandConverter::onOperationModeState(const OperationModeState::ConstSharedPtr msg)
+{
+  latest_operation_mode_state_ = msg;
+}
+
 std::shared_ptr<IFConverterDataPipeline> VtlCommandConverter::converterPipeline()
 {
   return converter_pipeline_;
@@ -77,7 +87,7 @@ std::shared_ptr<InterfaceConverterMultiMap> VtlCommandConverter::createConverter
     } else if (orig_elem.state == MainInputCommand::FINALIZED) {
       continue;
     }
-    const auto converter(new InterfaceConverter(orig_elem, node_));
+    const auto converter(new InterfaceConverter(orig_elem, node_, latest_operation_mode_state_));
     if (!converter->vtlAttribute()) {
       RCLCPP_DEBUG(node_->get_logger(),
         "VtlCommandConverter:%s: invalid vtl attribute.", __func__);