diff --git a/openarm_bringup/scripts/motor_disable_service.py b/openarm_bringup/scripts/motor_disable_service.py index 959b96b..466a257 100755 --- a/openarm_bringup/scripts/motor_disable_service.py +++ b/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.label = "inactive" + request.target_state.id = state_id + 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