Add auto operator change option to engage (autowarefoundation#10)

BalajiEdutech · Jan 19, 2022 · d722f7d · d722f7d
1 parent c5ba9b9
commit d722f7d
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git 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<tier4_external_api_msgs::srv::Engage>(
     "/api/autoware/set/engage",
     rmw_qos_profile_services_default);
+  cli_set_operator_ = proxy.create_client<tier4_external_api_msgs::srv::SetOperator>(
+    "/api/autoware/set/operator",
+    rmw_qos_profile_services_default);
   pub_engage_status_ = create_publisher<tier4_external_api_msgs::msg::EngageStatus>(
     "/api/external/get/engage", rclcpp::QoS(1));
   sub_engage_status_ = create_subscription<autoware_auto_vehicle_msgs::msg::Engage>(
@@ -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<tier4_external_api_msgs::srv::SetOperator::Request>();
+    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
@@ -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,19 +34,22 @@ 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;
 
   // ros interface
   rclcpp::CallbackGroup::SharedPtr group_;
   tier4_api_utils::Service<ExternalEngage>::SharedPtr srv_engage_;
   tier4_api_utils::Client<ExternalEngage>::SharedPtr cli_engage_;
+  tier4_api_utils::Client<SetOperator>::SharedPtr cli_set_operator_;
   rclcpp::Publisher<ExternalEngageStatus>::SharedPtr pub_engage_status_;
   rclcpp::Subscription<VehicleEngageStatus>::SharedPtr sub_engage_status_;
   rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_;
 
   // class state
   bool waiting_for_engage_;
+  bool auto_operator_change_;
 
   // ros callback
   void setEngage(