diff --git a/autoware_iv_external_api_adaptor/src/engage.cpp b/autoware_iv_external_api_adaptor/src/engage.cpp index 84406cca932..5a8df7b8739 100644 --- a/autoware_iv_external_api_adaptor/src/engage.cpp +++ b/autoware_iv_external_api_adaptor/src/engage.cpp @@ -32,6 +32,9 @@ Engage::Engage(const rclcpp::NodeOptions & options) cli_engage_ = proxy.create_client( "/api/autoware/set/engage", rmw_qos_profile_services_default); + cli_set_operator_ = proxy.create_client( + "/api/autoware/set/operator", + rmw_qos_profile_services_default); pub_engage_status_ = create_publisher( "/api/external/get/engage", rclcpp::QoS(1)); sub_engage_status_ = create_subscription( @@ -42,6 +45,7 @@ Engage::Engage(const rclcpp::NodeOptions & options) std::bind(&Engage::onAutowareState, this, _1)); waiting_for_engage_ = false; + auto_operator_change_ = declare_parameter("auto_operator_change", false); } void Engage::setEngage( @@ -53,6 +57,17 @@ void Engage::setEngage( return; } + if (auto_operator_change_) { + using tier4_external_api_msgs::msg::Operator; + auto req = std::make_shared(); + req->mode.mode = request->engage ? Operator::AUTONOMOUS : Operator::DRIVER; + auto [status, resp] = cli_set_operator_->call(req); + if (!tier4_api_utils::is_success(status)) { + response->status = status; + return; + } + } + auto [status, resp] = cli_engage_->call(request); if (!tier4_api_utils::is_success(status)) { response->status = status; diff --git a/autoware_iv_external_api_adaptor/src/engage.hpp b/autoware_iv_external_api_adaptor/src/engage.hpp index 10fb7399a4f..1058972bf33 100644 --- a/autoware_iv_external_api_adaptor/src/engage.hpp +++ b/autoware_iv_external_api_adaptor/src/engage.hpp @@ -20,6 +20,7 @@ #include "autoware_auto_system_msgs/msg/autoware_state.hpp" #include "autoware_auto_vehicle_msgs/msg/engage.hpp" #include "tier4_external_api_msgs/srv/engage.hpp" +#include "tier4_external_api_msgs/srv/set_operator.hpp" #include "tier4_external_api_msgs/msg/engage_status.hpp" namespace external_api @@ -33,6 +34,7 @@ class Engage : public rclcpp::Node private: using ExternalEngage = tier4_external_api_msgs::srv::Engage; using ExternalEngageStatus = tier4_external_api_msgs::msg::EngageStatus; + using SetOperator = tier4_external_api_msgs::srv::SetOperator; using VehicleEngageStatus = autoware_auto_vehicle_msgs::msg::Engage; using AutowareState = autoware_auto_system_msgs::msg::AutowareState; @@ -40,12 +42,14 @@ class Engage : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_; tier4_api_utils::Service::SharedPtr srv_engage_; tier4_api_utils::Client::SharedPtr cli_engage_; + tier4_api_utils::Client::SharedPtr cli_set_operator_; rclcpp::Publisher::SharedPtr pub_engage_status_; rclcpp::Subscription::SharedPtr sub_engage_status_; rclcpp::Subscription::SharedPtr sub_autoware_state_; // class state bool waiting_for_engage_; + bool auto_operator_change_; // ros callback void setEngage(