refactor(autoware_command_gate): extract ROS-free mode dispatch helper (#1145)

* refactor(autoware_command_gate): extract ROS-free mode dispatch helper

Move the request-to-mode dispatch (switch on the requested operation mode and
build the corresponding outputs) out of the node's service-callback lambda into a
ROS-free static helper CommandGateModeBuilder::dispatch_mode, which returns
std::optional<ModeOutputs> (std::nullopt for unknown/unsupported modes).

This makes the dispatch and its only error path (unknown mode -> PARAMETER_ERROR)
unit-testable with plain gtest, without spinning up rclcpp. Behavior is preserved:
the node still publishes outputs and sets status.success/code/message on the happy
path, and emits success=false with PARAMETER_ERROR on an unknown mode.

Add unit tests for dispatch_mode covering all four valid modes and the previously
untested unknown-mode nullopt branch.

The public node API is unchanged; dispatch_mode is an additive static method.

Refs: #1096
Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>

* refactor(autoware_command_gate): address review feedback

Move autoware_system_msgs/srv/change_operation_mode include from the
builder header to the .cpp where ChangeOperationMode::Request is used,
decoupling includers from the service definition.

Assert stamp propagation in DispatchModeAutonomous/Local/Remote tests
by passing non-zero stamps and checking outputs->state.stamp.

Refs: #1096
Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>

* refactor(autoware_command_gate): split mode-output and response builders (#80)

Make the ChangeOperationMode service callback route through two pure
message-in -> message-out builders: create_mode_output() builds the
messages to publish (std::nullopt for unknown modes) and create_response()
builds the service response. Both are ROS-node-free static functions that
take the request, so they are unit-testable in isolation. Behavior is
preserved; the publish-only ModeOutputs no longer carries response status.

Refs: #1096

Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>

---------

Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Co-authored-by: Junya Sasaki <j2sasaki1990@gmail.com>

Author: youtalk

control/autoware_command_gate/CMakeLists.txt

@@ -34,6 +34,8 @@ if(BUILD_TESTING)
   )
   ament_target_dependencies(test_command_gate_mode_builder
     autoware_adapi_v1_msgs
+    autoware_common_msgs
+    autoware_system_msgs
     autoware_vehicle_msgs
     builtin_interfaces
   )

control/autoware_command_gate/src/autoware_command_gate.cpp

@@ -20,8 +20,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 
-#include <autoware_adapi_v1_msgs/msg/response_status.hpp>
-#include <autoware_common_msgs/msg/response_status.hpp>
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 
 #include <rmw/types.h>
@@ -77,32 +75,11 @@ class AutowareCommandGateNode : public rclcpp::Node
         const SystemChangeOperationMode::Service::Request::SharedPtr req,
         const SystemChangeOperationMode::Service::Response::SharedPtr res) {
         const builtin_interfaces::msg::Time stamp = now();
-        ModeOutputs outputs;
-
-        switch (req->mode) {
-          case SystemChangeOperationMode::Service::Request::STOP:
-            outputs = mode_builder_.make_stop(stamp);
-            break;
-          case SystemChangeOperationMode::Service::Request::AUTONOMOUS:
-            outputs = mode_builder_.make_autonomous(stamp);
-            break;
-          case SystemChangeOperationMode::Service::Request::LOCAL:
-            outputs = mode_builder_.make_local(stamp);
-            break;
-          case SystemChangeOperationMode::Service::Request::REMOTE:
-            outputs = mode_builder_.make_remote(stamp);
-            break;
-          default:
-            res->status.success = false;
-            res->status.code = autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR;
-            res->status.message = "Unknown operation mode requested.";
-            return;
+        const auto outputs = CommandGateModeBuilder::create_mode_output(*req, stamp);
+        if (outputs) {
+          publish(*outputs);
         }
-
-        publish(outputs);
-        res->status.success = true;
-        res->status.code = 0;
-        res->status.message = outputs.status.message;
+        *res = CommandGateModeBuilder::create_response(*req, stamp);
       });
   }
 
@@ -121,7 +98,6 @@ class AutowareCommandGateNode : public rclcpp::Node
   rclcpp::Publisher<OperationModeSystemStateMsg>::SharedPtr system_state_pub_;
   rclcpp::Publisher<GearCommand>::SharedPtr gear_pub_;
   rclcpp::Service<SystemChangeOperationMode::Service>::SharedPtr srv_system_mode_;
-  CommandGateModeBuilder mode_builder_;
 };
 
 }  // namespace autoware::control::command_gate

control/autoware_command_gate/src/command_gate_mode_builder.cpp

@@ -14,17 +14,76 @@
 
 #include "command_gate_mode_builder.hpp"
 
+#include <autoware_common_msgs/msg/response_status.hpp>
+
+#include <optional>
 #include <string>
 
 namespace autoware::control::command_gate
 {
 
+std::optional<ModeOutputs> CommandGateModeBuilder::create_mode_output(
+  const Request & request, const builtin_interfaces::msg::Time & stamp)
+{
+  return dispatch_mode(request.mode, stamp);
+}
+
+CommandGateModeBuilder::Response CommandGateModeBuilder::create_response(
+  const Request & request, const builtin_interfaces::msg::Time & /*stamp*/)
+{
+  Response response;
+  const auto message = success_message(request.mode);
+  if (!message) {
+    response.status.success = false;
+    response.status.code = autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR;
+    response.status.message = "Unknown operation mode requested.";
+    return response;
+  }
+
+  response.status.success = true;
+  response.status.code = 0;  // 0 represents success (no specific success code defined)
+  response.status.message = *message;
+  return response;
+}
+
+std::optional<std::string> CommandGateModeBuilder::success_message(uint16_t mode)
+{
+  switch (mode) {
+    case Request::STOP:
+      return "Switched to STOP";
+    case Request::AUTONOMOUS:
+      return "Switched to AUTONOMOUS";
+    case Request::LOCAL:
+      return "Switched to LOCAL";
+    case Request::REMOTE:
+      return "Switched to REMOTE";
+    default:
+      return std::nullopt;
+  }
+}
+
+std::optional<ModeOutputs> CommandGateModeBuilder::dispatch_mode(
+  uint16_t mode, const builtin_interfaces::msg::Time & stamp)
+{
+  switch (mode) {
+    case Request::STOP:
+      return make_stop(stamp);
+    case Request::AUTONOMOUS:
+      return make_autonomous(stamp);
+    case Request::LOCAL:
+      return make_local(stamp);
+    case Request::REMOTE:
+      return make_remote(stamp);
+    default:
+      return std::nullopt;
+  }
+}
+
 ModeOutputs CommandGateModeBuilder::make_stop(const builtin_interfaces::msg::Time & stamp)
 {
   ModeOutputs outputs;
   fill_state(outputs.state, autoware_adapi_v1_msgs::msg::OperationModeState::STOP, stamp);
   fill_gear(outputs.gear, autoware_vehicle_msgs::msg::GearCommand::PARK, stamp);
-  outputs.status = make_status("Switched to STOP");
   return outputs;
 }
 
@@ -33,7 +92,6 @@ ModeOutputs CommandGateModeBuilder::make_autonomous(const builtin_interfaces::ms
   ModeOutputs outputs;
   fill_state(outputs.state, autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS, stamp);
   fill_gear(outputs.gear, autoware_vehicle_msgs::msg::GearCommand::DRIVE, stamp);
-  outputs.status = make_status("Switched to AUTONOMOUS");
   return outputs;
 }
 
@@ -42,7 +100,6 @@ ModeOutputs CommandGateModeBuilder::make_local(const builtin_interfaces::msg::Ti
   ModeOutputs outputs;
   fill_state(outputs.state, autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL, stamp);
   fill_gear(outputs.gear, autoware_vehicle_msgs::msg::GearCommand::NONE, stamp);
-  outputs.status = make_status("Switched to LOCAL");
   return outputs;
 }
 
@@ -51,7 +108,6 @@ ModeOutputs CommandGateModeBuilder::make_remote(const builtin_interfaces::msg::T
   ModeOutputs outputs;
   fill_state(outputs.state, autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE, stamp);
   fill_gear(outputs.gear, autoware_vehicle_msgs::msg::GearCommand::NONE, stamp);
-  outputs.status = make_status("Switched to REMOTE");
   return outputs;
 }
 
@@ -78,14 +134,4 @@ void CommandGateModeBuilder::fill_gear(
   msg.command = command;
 }
 
-autoware_adapi_v1_msgs::msg::ResponseStatus CommandGateModeBuilder::make_status(
-  const std::string & message)
-{
-  autoware_adapi_v1_msgs::msg::ResponseStatus status;
-  status.success = true;
-  status.code = 0;  // 0 represents success (no specific success code defined)
-  status.message = message;
-  return status;
-}
-
 }  // namespace autoware::control::command_gate

control/autoware_command_gate/src/command_gate_mode_builder.hpp

@@ -18,9 +18,11 @@
 #include <builtin_interfaces/msg/time.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
-#include <autoware_adapi_v1_msgs/msg/response_status.hpp>
+#include <autoware_system_msgs/srv/change_operation_mode.hpp>
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 
+#include <cstdint>
+#include <optional>
 #include <string>
 
 namespace autoware::control::command_gate
@@ -30,25 +32,49 @@ struct ModeOutputs
 {
   autoware_adapi_v1_msgs::msg::OperationModeState state;
   autoware_vehicle_msgs::msg::GearCommand gear;
-  autoware_adapi_v1_msgs::msg::ResponseStatus status;
 };
 
+/// Pure (ROS-node-free) builders that convert a ChangeOperationMode request message into the
+/// messages the command gate emits. Every entry point is a plain message-in -> message-out
+/// function so it can be unit-tested without spinning up a ROS node, publisher or service.
 class CommandGateModeBuilder
 {
 public:
+  using Request = autoware_system_msgs::srv::ChangeOperationMode::Request;
+  using Response = autoware_system_msgs::srv::ChangeOperationMode::Response;
+
+  /// Build the messages to publish for the requested operation mode.
+  /// Returns std::nullopt for unknown/unsupported modes so the caller publishes nothing.
+  /// The accepted values mirror the constants of
+  /// autoware_system_msgs::srv::ChangeOperationMode::Request (STOP/AUTONOMOUS/LOCAL/REMOTE).
+  static std::optional<ModeOutputs> create_mode_output(
+    const Request & request, const builtin_interfaces::msg::Time & stamp);
+
+  /// Build the service response for the requested operation mode.
+  /// Reports success for known modes and a PARAMETER_ERROR for unknown ones, mirroring the
+  /// publish decision made by create_mode_output().
+  static Response create_response(
+    const Request & request, const builtin_interfaces::msg::Time & stamp);
+
   static ModeOutputs make_stop(const builtin_interfaces::msg::Time & stamp);
   static ModeOutputs make_autonomous(const builtin_interfaces::msg::Time & stamp);
   static ModeOutputs make_local(const builtin_interfaces::msg::Time & stamp);
   static ModeOutputs make_remote(const builtin_interfaces::msg::Time & stamp);
 
 private:
+  /// Map an operation-mode request value to its outputs, or std::nullopt for unknown modes.
+  static std::optional<ModeOutputs> dispatch_mode(
+    uint16_t mode, const builtin_interfaces::msg::Time & stamp);
+
+  /// Human-readable success message for a known mode, or std::nullopt for unknown modes.
+  static std::optional<std::string> success_message(uint16_t mode);
+
   static void fill_state(
     autoware_adapi_v1_msgs::msg::OperationModeState & msg, uint8_t mode,
     const builtin_interfaces::msg::Time & stamp);
   static void fill_gear(
     autoware_vehicle_msgs::msg::GearCommand & msg, uint8_t command,
     const builtin_interfaces::msg::Time & stamp);
-  static autoware_adapi_v1_msgs::msg::ResponseStatus make_status(const std::string & message);
 };
 
 }  // namespace autoware::control::command_gate