增加电机使能和失能服务

openarm_bringup/scripts/motor_disable_service.py

@@ -24,7 +24,7 @@ from std_srvs.srv import Trigger
 
 
 class MotorDisableServiceNode(Node):
-    """Expose a simple Trigger service to disable arm motors via ros2_control."""
+    """Expose Trigger services to disable/enable arm motors via ros2_control."""
 
     def __init__(self) -> None:
         super().__init__("motor_disable_service")
@@ -35,12 +35,16 @@ class MotorDisableServiceNode(Node):
             ["openarm_left_hardware_interface", "openarm_right_hardware_interface"],
         )
         self.declare_parameter("service_name", "disable_motors")
-        self.declare_parameter("service_timeout_sec", 3.0)
+        self.declare_parameter("enable_service_name", "enable_motors")
+        self.declare_parameter("service_timeout_sec", 1.0)
 
         controller_manager_name = (
             self.get_parameter("controller_manager_name").get_parameter_value().string_value
         )
         service_name = self.get_parameter("service_name").get_parameter_value().string_value
+        enable_service_name = (
+            self.get_parameter("enable_service_name").get_parameter_value().string_value
+        )
 
         self._component_names = self._read_component_names()
         self._service_timeout_sec = (
@@ -58,9 +62,10 @@ class MotorDisableServiceNode(Node):
         )
 
         self.create_service(Trigger, service_name, self._handle_disable_request)
+        self.create_service(Trigger, enable_service_name, self._handle_enable_request)
 
         self.get_logger().info(
-            f"Motor disable service ready: service='{service_name}', "
+            f"Motor services ready: disable='{service_name}', enable='{enable_service_name}', "
             f"cm='{controller_manager_name}', components={self._component_names}"
         )
 
@@ -101,30 +106,83 @@ class MotorDisableServiceNode(Node):
         return response
 
     def _set_component_inactive(self, component_name: str) -> bool:
+        return self._set_component_state(
+            component_name=component_name,
+            state_id=State.PRIMARY_STATE_INACTIVE,
+            state_label="inactive",
+            action_word="disable",
+            action_done_word="Disabled",
+        )
+
+    def _handle_enable_request(
+        self, _request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        if not self._component_names:
+            response.success = False
+            response.message = "No hardware_components configured."
+            return response
+
+        if not self._set_state_client.wait_for_service(timeout_sec=self._service_timeout_sec):
+            response.success = False
+            response.message = "controller_manager set_hardware_component_state service not available."
+            return response
+
+        failed_components = []
+        for component_name in self._component_names:
+            if not self._set_component_active(component_name):
+                failed_components.append(component_name)
+
+        if failed_components:
+            response.success = False
+            response.message = "Failed to enable components: " + ", ".join(failed_components)
+            return response
+
+        response.success = True
+        response.message = "Motors enabled for: " + ", ".join(self._component_names)
+        return response
+
+    def _set_component_active(self, component_name: str) -> bool:
+        return self._set_component_state(
+            component_name=component_name,
+            state_id=State.PRIMARY_STATE_ACTIVE,
+            state_label="active",
+            action_word="enable",
+            action_done_word="Enabled",
+        )
+
+    def _set_component_state(
+        self,
+        component_name: str,
+        state_id: int,
+        state_label: str,
+        action_word: str,
+        action_done_word: str,
+    ) -> bool:
         request = SetHardwareComponentState.Request()
         request.name = component_name
-        request.target_state.id = State.PRIMARY_STATE_INACTIVE
+        request.target_state.id = state_id
-        request.target_state.label = "inactive"
+        request.target_state.label = state_label
 
         future = self._set_state_client.call_async(request)
         rclpy.spin_until_future_complete(self, future, timeout_sec=self._service_timeout_sec)
 
         if not future.done() or future.result() is None:
             self.get_logger().error(
-                f"Timeout or empty response when disabling hardware component '{component_name}'."
+                f"Timeout or empty response when trying to {action_word} "
+                f"hardware component '{component_name}'."
             )
             return False
 
         result = future.result()
         if not result.ok:
             self.get_logger().error(
-                f"controller_manager rejected disable request for '{component_name}' "
+                f"controller_manager rejected {action_word} request for '{component_name}' "
                 f"(state={result.state.id}:{result.state.label})."
             )
             return False
 
         self.get_logger().info(
-            f"Disabled component '{component_name}' -> "
+            f"{action_done_word} component '{component_name}' -> "
             f"state={result.state.id}:{result.state.label}"
         )
         return True