Beta/2.0.0

vtl_adapter/README.md

@@ -9,7 +9,7 @@ In the v2i_interface, update the received common ros2 topic for VTL.
 
 - input
   - from [autoware.universe](https://github.com/autowarefoundation/autoware.universe/)
-    - `/awapi/tmp/infrastructure_commands` \[[tier4_v2x_msgs/msg/InfrastructureCommandArray](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_v2x_msgs/msg/InfrastructureCommandArray.msg)\]:<br>Control command to V2I infrastructure. It has an array structure to control multiple infrastructures at the same time.
+    - `/api/external/get/virtual_traffic_light/commands` \[[tier4_v2x_msgs/msg/InfrastructureCommandArray](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_v2x_msgs/msg/InfrastructureCommandArray.msg)\]:<br>Control command to V2I infrastructure. It has an array structure to control multiple infrastructures at the same time.
 - output
   - to [autoware.universe](https://github.com/autowarefoundation/autoware.universe/)
     - `/system/v2x/virtual_traffic_light_status` \[[tier4_v2x_msgs/msg/VirtualTrafficLightStateArray](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_v2x_msgs/msg/VirtualTrafficLightStateArray.msg)\]:<br>ROS2 interface from `v2i_status` (UDP).

vtl_adapter/include/vtl_adapter/eve_vtl_interface_converter.hpp

@@ -20,7 +20,11 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_v2x_msgs/msg/infrastructure_command.hpp"
 #include "tier4_v2x_msgs/msg/key_value.hpp"
-#include "autoware_state_machine_msgs/msg/state_machine.hpp"
+
+#include "autoware_adapi_v1_msgs/msg/route_state.hpp"
+#include "autoware_adapi_v1_msgs/msg/route.hpp"
+#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
+#include "eve_cmd_gate_msgs/msg/engage_request_state.hpp"
 
 #include "vtl_adapter/eve_vtl_attribute.hpp"
 
@@ -29,7 +33,12 @@ namespace eve_vtl_interface_converter
 
 using EveVTLAttr = eve_vtl_attribute::EveVTLAttr;
 using InfrastructureCommand = tier4_v2x_msgs::msg::InfrastructureCommand;
-using StateMachine = autoware_state_machine_msgs::msg::StateMachine;
+
+using RouteState = autoware_adapi_v1_msgs::msg::RouteState;
+using Route = autoware_adapi_v1_msgs::msg::Route;
+using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
+using RouteData = autoware_adapi_v1_msgs::msg::RouteData;
+using AutonomousDrivingStartButton =eve_cmd_gate_msgs::msg::EngageRequestState;
 
 class EveVTLInterfaceConverter
 {
@@ -39,18 +48,35 @@ class EveVTLInterfaceConverter
 
   const std::shared_ptr<EveVTLAttr>& vtlAttribute() const;
   const InfrastructureCommand& command() const;
-  std::optional<uint8_t> request(
-    const StateMachine::ConstSharedPtr& state) const;
+  std::optional<uint8_t> request() const;
   bool response(const uint8_t& response_bit) const;
 private:
   bool init(const InfrastructureCommand& input_command);
   std::string convertInfraCommand(const uint8_t& input_command) const;
-  std::optional<std::string> convertADState(
-    const StateMachine::ConstSharedPtr& state) const;
+  std::optional<std::string> convertADState() const;
+
+    // Subscription
+  rclcpp::Subscription<RouteState>::SharedPtr sub_routing_state_;
+  rclcpp::Subscription<Route>::SharedPtr sub_routing_route_;
+  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_state_;
+  rclcpp::Subscription<AutonomousDrivingStartButton>::SharedPtr sub_autonomous_driving_start_button_;
+
+  // Callback
+  void onState(const RouteState::ConstSharedPtr msg);
+  void onRoute(const Route::ConstSharedPtr msg);
+  void onOperationModeState(const OperationModeState::ConstSharedPtr msg);
+  void onAutonomousDrivingStartButton(const AutonomousDrivingStartButton::ConstSharedPtr msg);
 
   InfrastructureCommand command_;
   std::shared_ptr<EveVTLAttr> vtl_attr_;
   rclcpp::Node* node_;
+  uint16_t state_;
+  Route route_;
+  bool is_autoware_control_enabled_;
+  bool is_in_transition_;
+  uint8_t mode_;
+  bool is_accept_;
+  bool is_request_;
 };
 
 }  // namespace eve_vtl_interface_converter

vtl_adapter/include/vtl_adapter/vtl_command_converter.hpp

@@ -23,7 +23,6 @@
 #include "v2i_interface_msgs/msg/infrastructure_command_array.hpp"
 
 // sub input
-#include "autoware_state_machine_msgs/msg/state_machine.hpp"
 
 #include "vtl_adapter/interface_converter_data_pipeline.hpp"
 #include "vtl_adapter/eve_vtl_interface_converter.hpp"
@@ -37,8 +36,6 @@ using MainOutputCommandArr = v2i_interface_msgs::msg::InfrastructureCommandArray
 using MainOutputCommand = v2i_interface_msgs::msg::InfrastructureCommand;
 using InterfaceConverter = eve_vtl_interface_converter::EveVTLInterfaceConverter;
 
-using SubInputState = autoware_state_machine_msgs::msg::StateMachine;
-
 using InterfaceConverterMultiMap =
   std::unordered_multimap<uint8_t, std::shared_ptr<InterfaceConverter>>;
 using InterfaceConverterMap =
@@ -60,20 +57,13 @@ class VtlCommandConverter
   // Publisher
   rclcpp::Publisher<MainOutputCommandArr>::SharedPtr command_pub_;
 
-  // Subscription
-  rclcpp::Subscription<SubInputState>::SharedPtr state_sub_;
-
-  // Callback
-  void onState(const SubInputState::ConstSharedPtr msg);
-
   // Preprocess
   std::shared_ptr<InterfaceConverterMultiMap> createConverter(
     const MainInputCommandArr::ConstSharedPtr& original_command) const;
   std::optional<MainOutputCommandArr> requestCommand(
     const std::shared_ptr<InterfaceConverterMultiMap>& converter_multimap) const;
 
   //member variables
-  SubInputState::ConstSharedPtr state_;
   std::shared_ptr<IFConverterDataPipeline> converter_pipeline_;
 };
 

vtl_adapter/package.xml

@@ -30,7 +30,8 @@
   <depend>rclcpp_components</depend>
   <depend>tier4_v2x_msgs</depend>
   <depend>v2i_interface_msgs</depend>
-  <depend>autoware_state_machine_msgs</depend>
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>eve_cmd_gate_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

vtl_adapter/src/eve_vtl_interface_converter.cpp

@@ -28,11 +28,29 @@ Class public function
 ***************************************************************
 */
 
+
 EveVTLInterfaceConverter::EveVTLInterfaceConverter(
   const InfrastructureCommand& input_command, rclcpp::Node* node)
   : command_(input_command), node_(node)
 {
+  using namespace std::placeholders;
   init(input_command);
+  // Subscription
+  sub_routing_state_ = node->create_subscription<RouteState>(
+    "/api/routing/state", rclcpp::QoS{1}.transient_local(),
+    std::bind(&EveVTLInterfaceConverter::onState, this, std::placeholders::_1));
+
+  sub_routing_route_ = node->create_subscription<Route>(
+    "/api/routing/route", rclcpp::QoS{1}.transient_local(),
+    std::bind(&EveVTLInterfaceConverter::onRoute, this, std::placeholders::_1));
+
+  sub_operation_mode_state_ = node->create_subscription<OperationModeState>(
+    "/api/operation_mode/state", rclcpp::QoS(1).transient_local(),
+    std::bind(&EveVTLInterfaceConverter::onOperationModeState, this, _1));
+
+ sub_autonomous_driving_start_button_ = node->create_subscription<AutonomousDrivingStartButton>(
+    "/eve_cmd_gate/engage_request_state",rclcpp::QoS(1).transient_local(),
+    std::bind(&EveVTLInterfaceConverter::onAutonomousDrivingStartButton, this, _1)); 
 }
 
 const std::shared_ptr<EveVTLAttr>& EveVTLInterfaceConverter::vtlAttribute() const
@@ -45,8 +63,30 @@ const InfrastructureCommand& EveVTLInterfaceConverter::command() const
   return command_;
 }
 
-std::optional<uint8_t> EveVTLInterfaceConverter::request(
-    const StateMachine::ConstSharedPtr& state) const
+void EveVTLInterfaceConverter::onState(const RouteState::ConstSharedPtr msg)
+{
+  state_ = msg->state;
+}
+
+void EveVTLInterfaceConverter::onRoute(const Route::ConstSharedPtr msg)
+{
+  route_.data = msg->data;
+}
+
+void EveVTLInterfaceConverter::onOperationModeState(const OperationModeState::ConstSharedPtr msg)
+{
+  is_autoware_control_enabled_ = msg->is_autoware_control_enabled;
+  is_in_transition_ = msg->is_in_transition;
+  mode_ = msg->mode;
+}
+
+void EveVTLInterfaceConverter::onAutonomousDrivingStartButton(const AutonomousDrivingStartButton::ConstSharedPtr msg)
+{
+  is_accept_ = msg ->is_engage_accepted;
+  is_request_ = msg ->is_engage_requesting;
+}
+
+std::optional<uint8_t> EveVTLInterfaceConverter::request() const
 {
   if (!vtl_attr_) {
     RCLCPP_WARN_THROTTLE(
@@ -55,8 +95,8 @@ std::optional<uint8_t> EveVTLInterfaceConverter::request(
     return std::nullopt;
   }
   const auto command_str = convertInfraCommand(command_.state);
-  const auto state_str = convertADState(state);
-  return vtl_attr_->request(command_str, state_str);
+  const auto state_str = convertADState();
+  return vtl_attr_ ->request(command_str, state_str);
 }
 
 bool EveVTLInterfaceConverter::response(const uint8_t& response_bit) const
@@ -249,8 +289,7 @@ std::string EveVTLInterfaceConverter::convertInfraCommand(const uint8_t& input_c
     (eve_vtl_spec::VALUE_SECTION_REQ) : (eve_vtl_spec::VALUE_SECTION_NULL);
 }
 
-std::optional<std::string>
-  EveVTLInterfaceConverter::convertADState(const StateMachine::ConstSharedPtr& state) const
+std::optional<std::string> EveVTLInterfaceConverter::convertADState() const
 {
   if (!vtl_attr_) {
     RCLCPP_WARN_THROTTLE(
@@ -266,31 +305,33 @@ std::optional<std::string>
     return eve_vtl_spec::VALUE_PERMIT_STATE_NULL;
   }
   const auto permit_state = permit_state_opt.value();
-
-  if (!state) {
-    RCLCPP_WARN_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), ERROR_THROTTLE_MSEC,
-      "EveVTLInterfaceConverter::%s: state is null", __func__);
-    return std::nullopt;
-  }
-  const auto& srv_state = state->service_layer_state;
   bool is_valid_state = false;
-  if (permit_state == eve_vtl_spec::VALUE_PERMIT_STATE_DRIVING) {
-    const bool fill_lower_bound = (srv_state >= StateMachine::STATE_RUNNING);
-    const bool fill_upper_bound = (srv_state < StateMachine::STATE_ARRIVED_GOAL);
-    is_valid_state = (fill_lower_bound && fill_upper_bound);
-  }
-  else if (permit_state == eve_vtl_spec::VALUE_PERMIT_STATE_NULL) {
+  bool isReadyForDeparture_flg = false;
+  bool driving_flg = false;
+
+  if (state_ == autoware_adapi_v1_msgs::msg::RouteState::SET) {
+    if (route_.data.size() != 0) {
+      if (is_autoware_control_enabled_ && !is_in_transition_ ) {
+        if (mode_ != OperationModeState::AUTONOMOUS) {
+          if (is_accept_ || is_request_) {
+            isReadyForDeparture_flg = true;
+          }
+        } else {
+          driving_flg = true;
+        }
+      }
+    }
+    is_valid_state = (isReadyForDeparture_flg || driving_flg);
+  } else if (permit_state == eve_vtl_spec::VALUE_PERMIT_STATE_NULL) {
     is_valid_state = true;
   }
 
   if (!is_valid_state) {
     RCLCPP_WARN_STREAM_THROTTLE(
       node_->get_logger(), *node_->get_clock(), ERROR_THROTTLE_MSEC,
       "EveVTLInterfaceConverter::" << __func__ <<
-      ": state is invalid: " << srv_state);
+      ": state is invalid " );
   }
   return (is_valid_state) ? permit_state_opt : std::nullopt;
-}
-
-}  // namespace eve_vtl_interface_converter
+}  
+}// namespace eve_vtl_interface_converter

vtl_adapter/src/vtl_command_converter.cpp

@@ -36,11 +36,7 @@ void VtlCommandConverter::init(rclcpp::Node* node)
   auto subscriber_option = rclcpp::SubscriptionOptions();
   subscriber_option.callback_group = group;
 
-  // Subscription
-  state_sub_ = node->create_subscription<SubInputState>(
-    "/autoware_state_machine/state", 1,
-    std::bind(&VtlCommandConverter::onState, this, _1),
-    subscriber_option);
+
   // Publisher
   command_pub_ = node->create_publisher<MainOutputCommandArr>(
     "~/output/infrastructure_commands",
@@ -50,7 +46,6 @@ void VtlCommandConverter::init(rclcpp::Node* node)
     "VtlCommandConverter: initialized.");
 }
 
-
 std::shared_ptr<IFConverterDataPipeline> VtlCommandConverter::converterPipeline()
 {
   return converter_pipeline_;
@@ -69,11 +64,6 @@ void VtlCommandConverter::onCommand(const MainInputCommandArr::ConstSharedPtr ms
   converter_pipeline_->add(converter_multimap);
 }
 
-void VtlCommandConverter::onState(const SubInputState::ConstSharedPtr msg)
-{
-  state_ = msg;
-}
-
 std::shared_ptr<InterfaceConverterMultiMap> VtlCommandConverter::createConverter(
     const MainInputCommandArr::ConstSharedPtr& original_command) const
 {
@@ -116,7 +106,7 @@ std::optional<MainOutputCommandArr> VtlCommandConverter::requestCommand(
   }
   std::unordered_map<uint8_t, MainOutputCommand> command_map;
   for (const auto& [id, converter] : *converter_multimap) {
-    const auto& req = converter->request(state_);
+    const auto& req = converter->request();
     if (!req) {
       RCLCPP_DEBUG(node_->get_logger(),
         "VtlCommandConverter:%s: failed to request (id=%d).", __func__, id);