Enable gRPC-based servo controller switching

aegis_grpc/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(ament_cmake REQUIRED)
 set(ROS_DEPENDS
     rclcpp
     rclcpp_action
+    controller_manager_msgs
     control_msgs
     geometry_msgs
     sensor_msgs

aegis_grpc/include/aegis_grpc/robot_control_service.hpp

@@ -9,6 +9,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include <controller_manager_msgs/srv/switch_controller.hpp>
 #include <control_msgs/action/gripper_command.hpp>
 #include <control_msgs/msg/joint_jog.hpp>
 #include <geometry_msgs/msg/twist.hpp>
@@ -34,6 +35,16 @@ class RobotControlServiceImpl final
     explicit RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> node);
 
     // TODO(issue#87) Create additional methods to enable and disable servo control.
+    grpc::Status ServoEnable(
+        grpc::ServerContext* context,
+        const google::protobuf::Empty* request,
+        proto_aegis_grpc::v1::TriggerResponse* response) override;
+
+    grpc::Status ServoDisable(
+        grpc::ServerContext* context,
+        const google::protobuf::Empty* request,
+        proto_aegis_grpc::v1::TriggerResponse* response) override;
+
     grpc::Status ServoJoint(
         grpc::ServerContext* context,
         const proto_aegis_grpc::v1::JointJog* request,
@@ -70,17 +81,22 @@ class RobotControlServiceImpl final
         proto_aegis_grpc::v1::TriggerResponse* response) override;
 
   private:
+    bool SwitchControllers(
+        const std::vector<std::string>& activate,
+        const std::vector<std::string>& deactivate);
+
+    void ServoPublishLoop();
+    void GripperSendGoal(double position, double max_effort);
+
     rclcpp::Logger get_logger() const;
 
     template <class T>
     void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description);
 
-    void ServoPublishLoop();
-    void GripperSendGoal(double position, double max_effort);
-
     std::shared_ptr<rclcpp::Node> node_;
     std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
 
+    rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
     rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr servo_joint_pub_;
     rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_tcp_pub_;
     rclcpp_action::Client<GripperCommand>::SharedPtr gripper_client_;

aegis_grpc/package.xml

@@ -17,6 +17,7 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>control_msgs</depend>
+  <depend>controller_manager_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libgrpc</depend>
   <depend>moveit_msgs</depend>

aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto

@@ -21,6 +21,8 @@ message RobotState {
 }
 
 service RobotControlService {
+  rpc ServoEnable(google.protobuf.Empty) returns (TriggerResponse);
+  rpc ServoDisable(google.protobuf.Empty) returns (TriggerResponse);
   rpc ServoJoint(JointJog) returns (google.protobuf.Empty);
   rpc ServoTCP(Twist) returns (google.protobuf.Empty);
   rpc GotoPose(Pose) returns (TriggerResponse);

aegis_grpc/python_client/aegis_grpc_client/grpc_client.py

@@ -396,3 +396,19 @@ async def gripper_open(self) -> tuple[bool, str]:
         except grpc.RpcError as e:
             self.logger.error(f"GripperOpen failed: {e}")
             raise
+
+    async def servo_enable(self) -> tuple[bool, str]:
+        self._check_connected()
+        try:
+            resp = await self.control_stub.ServoEnable(Empty())
+            return resp.success, resp.msg
+        except grpc.RpcError as e:
+            raise RuntimeError("Failed to enable servo") from e
+
+    async def servo_disable(self) -> tuple[bool, str]:
+        self._check_connected()
+        try:
+            resp = await self.control_stub.ServoDisable(Empty())
+            return resp.success, resp.msg
+        except grpc.RpcError as e:
+            raise RuntimeError("Failed to disable servo") from e

aegis_grpc/src/robot_control_service.cpp

@@ -35,6 +35,7 @@ RobotControlServiceImpl::RobotControlServiceImpl(
   servo_frequency_ratio_ = std::round(servo_out_hz / servo_in_hz);
   RCLCPP_INFO(get_logger(), "> Frequency ratio for servo re-publishig is %i", servo_frequency_ratio_);
 
+  switch_controller_client_ = node_->create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
   servo_joint_pub_ = node_->create_publisher<control_msgs::msg::JointJog>(
       node_->get_parameter("topic_servo_joint").as_string(), 10);
   servo_tcp_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>(
@@ -75,6 +76,86 @@ void RobotControlServiceImpl::DeclareROSParameter(
               p.value_to_string().c_str());
 }
 
+bool RobotControlServiceImpl::SwitchControllers(
+  const std::vector<std::string>& activate,
+  const std::vector<std::string>& deactivate) {
+  auto req = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+
+  req->activate_controllers = activate;
+  req->deactivate_controllers = deactivate;
+  req->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+
+  if (!switch_controller_client_->wait_for_service(action_timeout_)) {
+    RCLCPP_WARN(get_logger(), "Switch controller service not available");
+    return false;
+  }
+
+  auto future = switch_controller_client_->async_send_request(req);
+  auto status = future.wait_for(action_timeout_);
+
+  if (status != std::future_status::ready || !future.valid()) {
+    RCLCPP_WARN(get_logger(), "Switch controller call failed or timed out.");
+    return false;
+  }
+
+  return future.get()->ok;
+}
+
+grpc::Status RobotControlServiceImpl::ServoEnable(
+  grpc::ServerContext*,
+  const google::protobuf::Empty*,
+  proto_aegis_grpc::v1::TriggerResponse* response)
+{
+  response->set_success(false);
+
+  if (!SwitchControllers(
+        {"forward_position_controller"},
+        {"scaled_joint_trajectory_controller"})) {
+    std::string err = "Controller switch failed.";
+    RCLCPP_WARN(get_logger(), "[RobotControlService][ServoEnable] %s", err.c_str());
+    response->set_msg(err);
+    return grpc::Status(grpc::StatusCode::INTERNAL, err);
+  }
+
+  {
+    std::lock_guard<std::mutex> lock(servo_mutex_);
+    servo_mode_ = ServoMode::JointJog;
+  }
+
+  response->set_success(true);
+  response->set_msg("");
+  return grpc::Status::OK;
+}
+
+grpc::Status RobotControlServiceImpl::ServoDisable(
+  grpc::ServerContext*,
+  const google::protobuf::Empty*,
+  proto_aegis_grpc::v1::TriggerResponse* response)
+{
+  response->set_success(false);
+
+  if (!SwitchControllers(
+        {"scaled_joint_trajectory_controller"},
+        {"forward_position_controller"})) {
+
+    std::string err = "Controller switch failed.";
+    RCLCPP_WARN(get_logger(), "[RobotControlService][ServoDisable] %s", err.c_str());
+    response->set_msg(err);
+    return grpc::Status(grpc::StatusCode::INTERNAL, err);
+  }
+
+  {
+    std::lock_guard<std::mutex> lock(servo_mutex_);
+    servo_mode_ = ServoMode::None;
+    servo_msgs_left_ = 0;
+  }
+
+  response->set_success(true);
+  response->set_msg("");
+  RCLCPP_WARN(get_logger(), "[RobotControlService][ServoDisable] RETURN");
+  return grpc::Status::OK;
+}
+
 void RobotControlServiceImpl::ServoPublishLoop() {
   static ServoMode mode;
   static control_msgs::msg::JointJog jog_msg;