[Draft] Refactor servo architecture

src/servo_pkg/config/parent_config.yaml

@@ -0,0 +1,23 @@
+Parent_config:
+    ros__parameters:
+      # this is the highest servo number
+      max_num_servo: 3
+      servo_num: 1
+      # Tilt
+      servo0:
+        name: "tilt"
+        min: 350.0
+        max: 2500.0
+        rom: 6.2832
+      # PAN
+      servo1:
+        name: "pan"
+        min: 350.0
+        max: 2500.0
+        rom: 6.2832
+      # GRIPPER
+      servo3:
+        name: "gripper"
+        min: 500.0
+        max: 2500.0
+        rom: 3.1415

src/servo_pkg/config/pi_controller.yaml

@@ -1,16 +1,19 @@
 pi_Servo_node:
   ros__parameters:
-    servos_used: 2
+    # this is the highest servo number
+    max_num_servo: 1
     # Microscope Servo
     servo0:
+      name: "microscope"
       out_pin: 18
       min: 2.5   # Min PWM value
       max: 12.5  # Max PWM value
-      rom: 180   # 180 degrees
+      rom: 3.1415   # 180 degrees
 
     # Collection Servo
     servo1:
+      name: "collection"
       out_pin: 19
       min: 4.5   # Min PWM value
       max: 10.5  # Max PWM value
-      rom: 180   # 180 degrees
+      rom: 3.1415   # 180 degrees

src/servo_pkg/config/servo_client.yaml

@@ -0,0 +1,11 @@
+Servo_Client_node:
+  ros__parameters:
+    servo_num: 1
+    servo0:
+      name: "tilt"
+    # PAN
+    servo1:
+      name: "pan"
+    # GRIPPER
+    servo3:
+      name: "gripper"

src/servo_pkg/config/usb_controller.yaml

@@ -1,18 +1,24 @@
 USB_Servo_node:
   ros__parameters:
+    # this is the highest servo number
+    max_num_servo: 3
+    servo_num: 1
     serial_port: "/dev/serial/by-id/usb-Pololu_Corporation_Pololu_Mini_Maestro_12-Channel_USB_Servo_Controller_00460938-if00"
     # Tilt
-    port0:
-      min: 350
-      max: 2500
-      rom: 360
+    servo0:
+      name: "tilt"
+      min: 350.0
+      max: 2500.0
+      rom: 6.2832
     # PAN
-    port1:
-      min: 350
-      max: 2500
-      rom: 360
+    servo1:
+      name: "pan"
+      min: 350.0
+      max: 2500.0
+      rom: 6.2832
     # GRIPPER
-    port3:
-      min: 500
-      max: 2500
-      rom: 180
+    servo3:
+      name: "gripper"
+      min: 500.0
+      max: 2500.0
+      rom: 3.1415

src/servo_pkg/launch/i2c_servo_launch.launch.py

@@ -3,12 +3,15 @@
 
 
 def generate_launch_description():
+    parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml")
+
     return launch.LaunchDescription(
         [
             launch_ros.actions.Node(
                 package="servo_pkg",
                 executable="i2c_Servo",
                 name="USB_Servo_node",
+                parameters=[parent_params],
             ),
             launch_ros.actions.Node(
                 package="servo_pkg", executable="servo_client", name="servo_client_node"

src/servo_pkg/launch/pi_servo_launch.launch.py

@@ -6,15 +6,16 @@
 
 def generate_launch_description():
     pkg_servo = get_package_share_directory("servo_pkg")
-    parameters_file = os.path.join(pkg_servo, "config", "pi_controller.yaml")
+    child_params = os.path.join(pkg_servo, "config", "pi_controller.yaml")
+    parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml")
 
     return launch.LaunchDescription(
         [
             launch_ros.actions.Node(
                 package="servo_pkg",
                 executable="pi_Servo",
                 name="pi_Servo_node",
-                parameters=[parameters_file],
+                parameters=[parent_params, child_params],
                 remappings=[("servo_service", "science_servo_service")],
             ),
             # for example client

src/servo_pkg/launch/usb_servo_launch.launch.py

@@ -6,21 +6,23 @@
 
 def generate_launch_description():
     pkg_servo = get_package_share_directory("servo_pkg")
-    parameters_file = os.path.join(pkg_servo, "config", "usb_controller.yaml")
+    child_params = os.path.join(pkg_servo, "config", "usb_controller.yaml")
+    parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml")
+    client_params = os.path.join(pkg_servo, "config", "servo_client.yaml")
 
     return launch.LaunchDescription(
         [
             launch_ros.actions.Node(
                 package="servo_pkg",
                 executable="USB_Servo",
                 name="USB_Servo_node",
-                parameters=[parameters_file],
+                parameters=[parent_params, child_params],
+            ),
+            launch_ros.actions.Node(
+                package="servo_pkg",
+                executable="servo_client",
+                name="Servo_Client_node",
+                parameters=[client_params],
             ),
-            # for example client
-            # launch_ros.actions.Node(
-            #     package="servo_pkg",
-            #     executable="servo_client",
-            #     name="servo_client_node",
-            # ),
         ]
     )

src/servo_pkg/servo_pkg/USB_Servo.py

@@ -1,121 +1,81 @@
 import rclpy
-from rclpy.node import Node
-from interfaces.srv import MoveServo
+from std_msgs.msg import Float32
 from servo_pkg import maestro
+from servo_pkg.parent_config import Parent_Config
+from servo_pkg.parent_config import Servo_Info
 
-NUM_PORTS = 12
-DEFAULT_MIN = 512
-DEFAULT_MAX = 2400
-DEFAULT_MAX_DEGREES = 180
 
-
-class Servo_Info:
-    def __init__(self, port: int, min_us: int, max_us: int, max_deg: int):
-        self.port = port
-        self.min = min_us
-        self.max = max_us
-        self.rom = max_deg
-
-
-def convert_from_degrees(degrees: int, servo_info: Servo_Info) -> int:
+def convert_from_radians(angle, servo_info):
     total_range = servo_info.max - servo_info.min
-    return int(servo_info.min + (total_range * degrees / servo_info.rom))
+    return int(servo_info.min + (total_range * angle / servo_info.rom))
 
 
-def convert_to_degrees(value: int, servo_info: Servo_Info) -> int:
+def convert_to_radians(value, servo_info):
     total_range = servo_info.max - servo_info.min
-    return int(servo_info.rom * (value - servo_info.min) / total_range)
+    return servo_info.rom * (value - servo_info.min) / total_range
 
 
-class USB_Servo(Node):
+class USB_Servo(Parent_Config):
     def __init__(self):
         super().__init__("usb_servo")
 
+        # port parameter
         self.declare_parameter("serial_port", "/dev/ttyACM0")
         serial_port = (
             self.get_parameter("serial_port").get_parameter_value().string_value
         )
-        self.servo = maestro.Controller(serial_port)
-        self.srv = self.create_service(MoveServo, "servo_service", self.set_position)
-
-        self.servo_ranges = {}
-        self.load_port_config()
-
-        for port, servo in self.servo_ranges.items():
-            self.servo.setRange(port, servo.min, servo.max)
-
-    def load_port_config(self):
-        for port_number in range(NUM_PORTS):
-            self.declare_parameter(f"port{port_number}.min", DEFAULT_MIN)
-            min_us = (
-                self.get_parameter(f"port{port_number}.min")
-                .get_parameter_value()
-                .integer_value
-            )
-            self.declare_parameter(f"port{port_number}.max", DEFAULT_MAX)
-            max_us = (
-                self.get_parameter(f"port{port_number}.max")
-                .get_parameter_value()
-                .integer_value
-            )
-            self.declare_parameter(f"port{port_number}.rom", DEFAULT_MAX_DEGREES)
-            rom = (
-                self.get_parameter(f"port{port_number}.rom")
-                .get_parameter_value()
-                .integer_value
-            )
-            # Convert microseconds to quarter-microseconds
-            min_qus = min_us * 4
-            max_qus = max_us * 4
+        self.servo_controller = maestro.Controller(serial_port)
+        self.get_logger().info(f"{self.servo_info[self.servo_num].motor_name}")
+        self.sub = self.create_subscription(
+            Float32,
+            f"{self.servo_info[self.servo_num].motor_name}",
+            self.set_position,
+            3,
+        )
 
-            self.servo_ranges[port_number] = Servo_Info(
-                port_number, min_qus, max_qus, rom
-            )
-            self.get_logger().info(
-                f"Port {port_number} -> Min: {min_qus}, Max: {max_qus}"
-            )
+        self.set_range()
+        for port, servo in self.servo_info.items():
+            self.servo_controller.setRange(port, servo.min, servo.max)
 
-    def set_position(self, request: MoveServo, response: MoveServo) -> MoveServo:
-        port = request.port
-        if port not in self.servo_ranges:
-            response.status = False
-            response.status_msg = f"Invalid port: {port}"
-            return response
-
-        servo_info = self.servo_ranges[port]
-
-        # Update range if explicitly given
-        if (
-            request.min is not None
-            and request.min >= 0
-            and request.min < request.max
-            and request.max is not None
-            and request.max > 0
-        ):
-            servo_info.min = request.min
-            self.servo.setRange(port, servo_info.min, servo_info.max)
-            self.get_logger().info(
-                f"Updated range for port {port}: Min: {servo_info.min}, Max: {servo_info.max}"
-            )
+    def set_range(self):
+        for port in self.servo_info:
+            # Convert microseconds to quarter-microseconds
+            min_qus = self.servo_info[port].min * 4
+            max_qus = self.servo_info[port].max * 4
+            self.get_logger().info(f"Port {port} -> Min: {min_qus}, Max: {max_qus}")
+            self.servo_controller.setRange(port, min_qus, max_qus)
+
+    def set_position(self, msg):
+        port = self.servo_num
+        self.get_logger().info(f"Port is {port}")
+        self.check_valid_servo(port)
+        servo_info = self.servo_info[port]
+        total_range = servo_info.max - servo_info.min
+        self.get_logger().info(f"Float is {msg.data}")
+        self.get_logger().info(f"Total Range: {total_range}")
+        target_value = convert_from_radians(msg.data, servo_info)
+        self.get_logger().info(f"PWM target is {target_value}")
+        self.get_logger().info(f"Target value: {target_value}")
+        current_position = convert_to_radians(
+            self.servo_controller.getPosition(port), servo_info
+        )
 
-        target_value = convert_from_degrees(request.pos, servo_info)
-        current_position = convert_to_degrees(self.servo.getPosition(port), servo_info)
+        self.get_logger().info(f"Total Range: {total_range}")
+        self.get_logger().info(f"Radian value: {current_position}")
 
         if not (servo_info.min <= target_value <= servo_info.max):
-            response.status = False
-            response.status_msg = f"Servo {port} input out of range.\nCurrent position: {current_position}"
+            self.get_logger().warning(
+                f"Servo {port} input out of range.\nCurrent position: {current_position}"
+            )
         else:
             self.get_logger().debug(
-                f"Received request for port {port}: {request.pos} degrees -> {target_value}"
+                f"Received request for port {port}: {msg.data} angle -> {target_value}"
             )
-            self.servo.setTarget(port, target_value)
-            response.status = True
-            current_position = convert_to_degrees(
-                self.servo.getPosition(port), servo_info
+            self.servo_controller.setTarget(port, target_value)
+            current_position = convert_to_radians(
+                self.servo_controller.getPosition(port), servo_info
             )
-            response.status_msg = f"Servo {port} moved to: {current_position} degrees"
-
-        return response
+            self.get_logger().info(f"Servo {port} moved to angle: {current_position}")
 
 
 def main(args=None):