feat(agv_pro_base,agv_pro_msgs): add LED color and mode services with driver support

agv_pro_msgs/CMakeLists.txt

@@ -17,6 +17,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AGVProStatus.msg"
   "srv/SetDigitalOutput.srv"
   "srv/GetDigitalInput.srv"
+  "srv/SetLedColor.srv"
+  "srv/SetLedMode.srv"
   DEPENDENCIES std_msgs
 )
 

agv_pro_msgs/scripts/ros_client.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+import rclpy
+import time
+from rclpy.node import Node
+
+from agv_pro_msgs.srv import (
+    SetDigitalOutput,
+    GetDigitalInput,
+    SetLedColor,
+    SetLedMode
+)
+
+class AGVIOClient(Node):
+    def __init__(self):
+        super().__init__('agv_io_client')
+
+        # Create service client
+        self.cli_set_io = self.create_client(SetDigitalOutput, 'set_digital_output')
+        self.cli_get_io = self.create_client(GetDigitalInput, 'get_digital_input')
+        self.cli_led_output = self.create_client(SetLedColor, 'set_led_color')
+        self.cli_led_mode = self.create_client(SetLedMode, 'set_led_mode')
+
+        # Wait until all services are available
+        self._wait_for_services()
+
+    def _wait_for_services(self):
+        """Wait for all services to become available."""
+        clients = [
+            self.cli_set_io,
+            self.cli_get_io,
+            self.cli_led_output,
+            self.cli_led_mode
+        ]
+
+        for cli in clients:
+            while not cli.wait_for_service(timeout_sec=1.0):
+                pass
+
+    def _call_service(self, client, request):
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+
+        if future.result() is not None:
+            return future.result()
+        else:
+            self.get_logger().error(f'Service call failed: {future.exception()}')
+            return None
+
+    # ------------------------------
+    # Set digital output
+    # ------------------------------
+    def set_digital_output(self, pin: int, state: int) -> bool:
+        """Set digital output pin state."""
+        req = SetDigitalOutput.Request()
+        req.pin = pin
+        req.state = state
+
+        res = self._call_service(self.cli_set_io, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Get digital input
+    # ------------------------------
+    def get_digital_input(self, pin: int):
+        """Read digital input pin state."""
+        req = GetDigitalInput.Request()
+        req.pin = pin
+
+        res = self._call_service(self.cli_get_io, req)
+        return res.state if (res and res.success) else None
+
+    # ------------------------------
+    # Set LED color
+    # ------------------------------
+    def set_led_color(self,
+                      position: int,
+                      brightness: int,
+                      r: int,
+                      g: int,
+                      b: int) -> bool:
+        """Set LED RGB color and brightness."""
+        req = SetLedColor.Request()
+        req.position = position
+        req.brightness = brightness
+        req.r = r
+        req.g = g
+        req.b = b
+
+        res = self._call_service(self.cli_led_output, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Set LED mode
+    # ------------------------------
+    def set_led_mode(self, mode: bool) -> bool:
+        """Set LED mode (True/False)."""
+        req = SetLedMode.Request()
+        req.mode = mode
+
+        res = self._call_service(self.cli_led_mode, req)
+        return res.success if res else False
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    client = AGVIOClient()
+
+    ################
+    # Example usage:
+    ################
+
+    # client.set_digital_output(pin=1, state=1)
+    # client.get_digital_input(pin=2)
+
+    # client.set_led_mode(True)
+    # for i in range(10):
+    #     client.set_led_color(0, 100, 255, 255, 0)
+    #     client.set_led_color(1, 100, 255, 255, 0)
+    #     time.sleep(0.5)
+    #     client.set_led_color(0, 0, 0, 0, 0)
+    #     client.set_led_color(1, 0, 0, 0, 0)
+    #     time.sleep(0.5)
+
+    # client.set_led_color(0, 100, 255, 255, 0)
+    # client.set_led_color(1, 100, 255, 255, 0)
+    # client.set_led_mode(True)
+
+    client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_msgs/srv/SetLedColor.srv

@@ -0,0 +1,8 @@
+int32 position
+int32 brightness
+int32 r
+int32 g
+int32 b
+---
+bool success
+string message

agv_pro_msgs/srv/SetLedMode.srv

@@ -0,0 +1,4 @@
+bool mode
+---
+bool success
+string message