Merge pull request #5 from rogy-AquaLab/impl-communicate

NucleoとのUART実装

.github/workflows/build.yml

@@ -19,6 +19,7 @@ jobs:
       - uses: actions/checkout@v4
         with:
           path: src/2024_cavolinia
+      - run: sudo apt update
       - run: |
           . /opt/ros/iron/setup.bash
           rosdep update

device/nucleo_communicate_py/nucleo_communicate_py/main.py

@@ -9,12 +9,16 @@
 from .receiver import Receiver
 from .sender import Sender
 
+from .mutex_serial import MutexSerial
+
 
 def main(args=sys.argv):
     rclpy.init(args=args)
+    # FIXME: receiver.pyのコメント参照
+    mutex_serial = MutexSerial("/dev/ttyACM0")
     try:
-        sender = Sender()
-        receiver = Receiver()
+        sender = Sender(mutex_serial)
+        receiver = Receiver(mutex_serial)
 
         executor = SingleThreadedExecutor()
         executor.add_node(sender)

device/nucleo_communicate_py/nucleo_communicate_py/mutex_serial.py

@@ -0,0 +1,15 @@
+from contextlib import contextmanager
+from threading import Lock
+
+from serial import Serial
+
+
+class MutexSerial:
+    def __init__(self, *args, **kwargs) -> None:
+        self._lock = Lock()
+        self._ser = Serial(*args, **kwargs)
+
+    @contextmanager
+    def lock(self):
+        with self._lock:
+            yield self._ser

device/nucleo_communicate_py/nucleo_communicate_py/receiver.py

@@ -3,28 +3,71 @@
 import rclpy
 from rclpy.node import Node
 from packet_interfaces.msg import Current, Flex, Voltage
+from serial import Serial
+from std_msgs.msg import Header
+
+from .mutex_serial import MutexSerial
+
+
+class Recv:
+    def __init__(self, mutex_serial: MutexSerial) -> None:
+        self._mutex_serial = mutex_serial
+
+    # (flex1, flex2, current, voltage)
+    def receive_raw(self) -> tuple[int, int, int, int]:
+        with self._mutex_serial.lock() as serial:
+            assert isinstance(serial, Serial)
+            # Request sensor's data
+            serial.write(b"\x01")
+            # Receive nucleo's response
+            buf = serial.read(8)
+        # raw values
+        # FIXME: int.from_bytes使ったほうがいいかも
+        flex1 = (buf[0] << 0) | (buf[1] << 8)
+        flex2 = (buf[2] << 0) | (buf[3] << 8)
+        current = (buf[4] << 0) | (buf[5] << 8)
+        voltage = (buf[6] << 0) | (buf[7] << 8)
+        return (flex1, flex2, current, voltage)
+
+    def map_values(
+        flex1: int, flex2: int, current: int, voltage: int
+    ) -> tuple[int, int, float, float]:
+        # TODO: current, voltageの計算はnucleo側と要相談
+        return (flex1, flex2, current / 0xFFFF, voltage / 0xFFFF)
+
+    def receive(self) -> tuple[int, int, float, float]:
+        flex1, flex2, current, voltage = self.receive_raw()
+        return self.map_values(flex1, flex2, current, voltage)
 
 
 class Receiver(Node):
-    def __init__(self) -> None:
+    def __init__(self, mutex_serial: MutexSerial) -> None:
         super().__init__("receiver")
         self._current_publisher = self.create_publisher(Current, "current", 10)
         # NOTE: packet_interfaces/Composed の命名と揃える
         self._flex1_publisher = self.create_publisher(Flex, "flexsensor1", 10)
         self._flex2_publisher = self.create_publisher(Flex, "flexsensor2", 10)
         self._voltage_publisher = self.create_publisher(Voltage, "voltage", 10)
         self._timer = self.create_timer(0.5, self._timer_callback)
+        self._recv = Recv(mutex_serial)
 
     def _timer_callback(self) -> None:
-        # TODO
-        # https://github.com/rogy-AquaLab/2024_cavolinia_nucleo 参照
-        # UARTでNucleoからデータを取得してpublishする
-        self.get_logger().info("tick")
+        self.get_logger().trace("tick")
+        flex1, flex2, current, voltage = self._recv.receive_raw()
+        self.get_logger().info(f"received from nucleo: {flex1=}, {flex2=}, {current=}, {voltage=}")
+        flex1, flex2, current, voltage = self._recv.map_values(flex1, flex2, current, voltage)
+        self._flex1_publisher.publish(Flex(value=flex1))
+        self._flex2_publisher.publish(Flex(value=flex2))
+        # TODO: データのマッピングはnucleo側と相談
+        self._current_publisher.publish(Flex(value=current))
+        self._voltage_publisher.publish(Flex(value=voltage))
 
 
 def main(args=sys.argv):
     rclpy.init(args=args)
-    receiver = Receiver()
+    # FIXME: 手元のRaspberryPiだとこれで動くが...?
+    mutex_serial = MutexSerial("/dev/ttyACM0")
+    receiver = Receiver(mutex_serial)
     rclpy.spin(receiver)
     receiver.destroy_node()
     rclpy.shutdown()

device/nucleo_communicate_py/nucleo_communicate_py/sender.py

@@ -2,12 +2,59 @@
 
 import rclpy
 from rclpy.node import Node
+from serial import Serial
 from std_msgs.msg import Empty
 from packet_interfaces.msg import Power
 
+from .mutex_serial import MutexSerial
+
+
+class Sndr:
+    def __init__(self, mutex_serial: MutexSerial) -> None:
+        self._mutex_serial = mutex_serial
+
+    def send_power(self, power: Power) -> int:
+        buf = bytes([
+            # Sending nodification
+            0x00,
+            # BLDC 1
+            (power.bldc[0] >> 0) & 0xFF,
+            (power.bldc[0] >> 8) & 0xFF,
+            # BLDC 2
+            (power.bldc[1] >> 0) & 0xFF,
+            (power.bldc[1] >> 8) & 0xFF,
+            # BLDC 3
+            (power.bldc[2] >> 0) & 0xFF,
+            (power.bldc[2] >> 8) & 0xFF,
+            # BLDC 4
+            (power.bldc[3] >> 0) & 0xFF,
+            (power.bldc[3] >> 8) & 0xFF,
+            # Servo 1
+            (power.servo[0] >> 0) & 0xFF,
+            (power.servo[0] >> 8) & 0xFF,
+            # Servo 2
+            (power.servo[1] >> 0) & 0xFF,
+            (power.servo[1] >> 8) & 0xFF,
+            # Servo 3
+            (power.servo[2] >> 0) & 0xFF,
+            (power.servo[2] >> 8) & 0xFF,
+            # Servo 4
+            (power.servo[3] >> 0) & 0xFF,
+            (power.servo[3] >> 8) & 0xFF,
+        ])
+        with self._mutex_serial.lock() as serial:
+            assert isinstance(serial, Serial)
+            return serial.write(buf)
+
+    def send_quit(self) -> int:
+        buf = bytes([0xFF])
+        with self._mutex_serial.lock() as serial:
+            assert isinstance(serial, Serial)
+            return serial.write(buf)
+
 
 class Sender(Node):
-    def __init__(self) -> None:
+    def __init__(self, mutex_serial: MutexSerial) -> None:
         super().__init__("sender")
         self._quit_subscription = self.create_subscription(
             Empty,
@@ -21,23 +68,22 @@ def __init__(self) -> None:
             self._order_callback,
             10
         )
+        self._sender = Sndr(mutex_serial)
 
     def _quit_callback(self, _quit: Empty) -> None:
-        # TODO: UARTで書き込み
-        # https://github.com/rogy-AquaLab/2024_cavolinia_nucleo 参照
-        # [0xFF] を送る
-        self.get_logger().info("Received quit order")
+        self._sender.send_quit()
+        self.get_logger().info("Sent quit order")
 
-    def _order_callback(self, _order: Power) -> None:
-        # TODO
-        # https://github.com/rogy-AquaLab/2024_cavolinia_nucleo 参照
-        # [0x00, ...] を送る
-        self.get_logger().info("Received power order")
+    def _order_callback(self, order: Power) -> None:
+        self._sender.send_power(order)
+        self.get_logger().info("Sent power order")
 
 
 def main(args=sys.argv):
     rclpy.init(args=args)
-    sender = Sender()
+    # FIXME: receiver.pyのコメント参照
+    mutex_serial = MutexSerial("/dev/ttyACM0")
+    sender = Sender(mutex_serial)
     rclpy.spin(sender)
     sender.destroy_node()
     rclpy.shutdown()

device/nucleo_communicate_py/package.xml

@@ -8,6 +8,7 @@
   <license>MIT</license>
 
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>python3-serial</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>packet_interfaces</exec_depend>
 

packet/packet_interfaces/msg/Flex.msg

@@ -1,5 +1,5 @@
 std_msgs/Header header
 
 # nucleoから送られてきた値そのまま
-# TODO: 値の範囲は 0~0xFFFFFFFF で決まっているが、どちらが曲がっている側か決める
+# TODO: 値の範囲は 0~0xFFFF で決まっているが、どちらが曲がっている側か決める
 uint16 value