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);