diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e61d371..00d33d3 100644 --- a/.pre-commit-config.yaml +++ b/.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 diff --git a/v2i_interface/scripts/test/v2i_interface_test.py b/v2i_interface/scripts/test/v2i_interface_test.py index 9ca42d5..4afb066 100755 --- a/v2i_interface/scripts/test/v2i_interface_test.py +++ b/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) 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 87f11e1..6b5d9d6 100644 --- a/vtl_adapter/include/vtl_adapter/eve_vtl_interface_converter.hpp +++ b/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& vtlAttribute() const; const InfrastructureCommand& command() const; @@ -48,17 +49,11 @@ class EveVTLInterfaceConverter std::string convertInfraCommand(const uint8_t& input_command) const; std::optional convertADState() const; - // Subscription - rclcpp::Subscription::SharedPtr sub_operation_mode_state_; - - // Callback - void onOperationModeState(const OperationModeState::ConstSharedPtr msg); - // Member variables InfrastructureCommand command_; std::shared_ptr vtl_attr_; rclcpp::Node* node_; - uint8_t mode_; + OperationModeState::ConstSharedPtr operation_mode_state_ptr_; }; } // 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 e0acb21..ddc66b5 100644 --- a/vtl_adapter/include/vtl_adapter/vtl_command_converter.hpp +++ b/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>; 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 converterPipeline(); void onCommand(const MainInputCommandArr::ConstSharedPtr msg); private: + void onOperationModeState(const OperationModeState::ConstSharedPtr msg); + // Node rclcpp::Node* node_; // Publisher rclcpp::Publisher::SharedPtr command_pub_; + // Subscription: /api/operation_mode/state (single subscription, shared by all converters) + rclcpp::Subscription::SharedPtr sub_operation_mode_state_; + OperationModeState::ConstSharedPtr latest_operation_mode_state_; + // Preprocess std::shared_ptr createConverter( const MainInputCommandArr::ConstSharedPtr& original_command) const; diff --git a/vtl_adapter/src/eve_vtl_interface_converter.cpp b/vtl_adapter/src/eve_vtl_interface_converter.cpp index 4336b6d..b05c793 100644 --- a/vtl_adapter/src/eve_vtl_interface_converter.cpp +++ b/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( - "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), - std::bind(&EveVTLInterfaceConverter::onOperationModeState, this, _1)); } const std::shared_ptr& 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 EveVTLInterfaceConverter::request() const { if (!vtl_attr_) { @@ -279,15 +270,21 @@ std::optional 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(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; } diff --git a/vtl_adapter/src/vtl_command_converter.cpp b/vtl_adapter/src/vtl_command_converter.cpp index 58d2663..0706e2b 100644 --- a/vtl_adapter/src/vtl_command_converter.cpp +++ b/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( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&VtlCommandConverter::onOperationModeState, this, _1), + subscriber_option); // Publisher command_pub_ = node->create_publisher( @@ -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 VtlCommandConverter::converterPipeline() { return converter_pipeline_; @@ -77,7 +87,7 @@ std::shared_ptr 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__);