diff --git a/vtl_adapter/README.md b/vtl_adapter/README.md index a33fd25..0f3f209 100644 --- a/vtl_adapter/README.md +++ b/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)\]:
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)\]:
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)\]:
ROS2 interface from `v2i_status` (UDP). diff --git a/vtl_adapter/include/vtl_adapter/eve_vtl_interface_converter.hpp b/vtl_adapter/include/vtl_adapter/eve_vtl_interface_converter.hpp index 129a996..8835879 100644 --- a/vtl_adapter/include/vtl_adapter/eve_vtl_interface_converter.hpp +++ b/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& vtlAttribute() const; const InfrastructureCommand& command() const; - std::optional request( - const StateMachine::ConstSharedPtr& state) const; + std::optional 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 convertADState( - const StateMachine::ConstSharedPtr& state) const; + std::optional convertADState() const; + + // Subscription + rclcpp::Subscription::SharedPtr sub_routing_state_; + rclcpp::Subscription::SharedPtr sub_routing_route_; + rclcpp::Subscription::SharedPtr sub_operation_mode_state_; + rclcpp::Subscription::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 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 diff --git a/vtl_adapter/include/vtl_adapter/vtl_command_converter.hpp b/vtl_adapter/include/vtl_adapter/vtl_command_converter.hpp index 78c6538..e0acb21 100644 --- a/vtl_adapter/include/vtl_adapter/vtl_command_converter.hpp +++ b/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>; using InterfaceConverterMap = @@ -60,12 +57,6 @@ class VtlCommandConverter // Publisher rclcpp::Publisher::SharedPtr command_pub_; - // Subscription - rclcpp::Subscription::SharedPtr state_sub_; - - // Callback - void onState(const SubInputState::ConstSharedPtr msg); - // Preprocess std::shared_ptr createConverter( const MainInputCommandArr::ConstSharedPtr& original_command) const; @@ -73,7 +64,6 @@ class VtlCommandConverter const std::shared_ptr& converter_multimap) const; //member variables - SubInputState::ConstSharedPtr state_; std::shared_ptr converter_pipeline_; }; diff --git a/vtl_adapter/package.xml b/vtl_adapter/package.xml index c494fb1..56ddfc5 100644 --- a/vtl_adapter/package.xml +++ b/vtl_adapter/package.xml @@ -30,7 +30,8 @@ rclcpp_components tier4_v2x_msgs v2i_interface_msgs - autoware_state_machine_msgs + autoware_adapi_v1_msgs + eve_cmd_gate_msgs ament_cmake_gtest ament_lint_auto diff --git a/vtl_adapter/src/eve_vtl_interface_converter.cpp b/vtl_adapter/src/eve_vtl_interface_converter.cpp index 1d010bf..45aeb88 100644 --- a/vtl_adapter/src/eve_vtl_interface_converter.cpp +++ b/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( + "/api/routing/state", rclcpp::QoS{1}.transient_local(), + std::bind(&EveVTLInterfaceConverter::onState, this, std::placeholders::_1)); + + sub_routing_route_ = node->create_subscription( + "/api/routing/route", rclcpp::QoS{1}.transient_local(), + std::bind(&EveVTLInterfaceConverter::onRoute, this, std::placeholders::_1)); + + sub_operation_mode_state_ = node->create_subscription( + "/api/operation_mode/state", rclcpp::QoS(1).transient_local(), + std::bind(&EveVTLInterfaceConverter::onOperationModeState, this, _1)); + + sub_autonomous_driving_start_button_ = node->create_subscription( + "/eve_cmd_gate/engage_request_state",rclcpp::QoS(1).transient_local(), + std::bind(&EveVTLInterfaceConverter::onAutonomousDrivingStartButton, this, _1)); } const std::shared_ptr& EveVTLInterfaceConverter::vtlAttribute() const @@ -45,8 +63,30 @@ const InfrastructureCommand& EveVTLInterfaceConverter::command() const return command_; } -std::optional 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 EveVTLInterfaceConverter::request() const { if (!vtl_attr_) { RCLCPP_WARN_THROTTLE( @@ -55,8 +95,8 @@ std::optional 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 - EveVTLInterfaceConverter::convertADState(const StateMachine::ConstSharedPtr& state) const +std::optional EveVTLInterfaceConverter::convertADState() const { if (!vtl_attr_) { RCLCPP_WARN_THROTTLE( @@ -266,21 +305,24 @@ std::optional 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; } @@ -288,9 +330,8 @@ std::optional 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 diff --git a/vtl_adapter/src/vtl_command_converter.cpp b/vtl_adapter/src/vtl_command_converter.cpp index 371899b..58d2663 100644 --- a/vtl_adapter/src/vtl_command_converter.cpp +++ b/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( - "/autoware_state_machine/state", 1, - std::bind(&VtlCommandConverter::onState, this, _1), - subscriber_option); + // Publisher command_pub_ = node->create_publisher( "~/output/infrastructure_commands", @@ -50,7 +46,6 @@ void VtlCommandConverter::init(rclcpp::Node* node) "VtlCommandConverter: initialized."); } - std::shared_ptr 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 VtlCommandConverter::createConverter( const MainInputCommandArr::ConstSharedPtr& original_command) const { @@ -116,7 +106,7 @@ std::optional VtlCommandConverter::requestCommand( } std::unordered_map 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);