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