Merge pull request #12 from rogy-AquaLab/nucleo-channel

Add `Channel` Node

device/nucleo_communicate_py/README.md

@@ -8,15 +8,16 @@ TODO: いつかC++に書き換える
 
 - `Receiver`: Nucleoからデータを受け取る
 - `Sender`: Nucleoにデータを送る
+- `Channel`: データの送受信両方を行う
 
 ## Executables
 
 - `receiver`: `Receiver`Nodeをspin
 - `sender`: `Sender`Nodeをspin
-- `main`: `Receiver`, `Sender`Node2つを同時にspin
+- `channel`: `Channel`Nodeをspin
 
 ## Launches
 
-- `main_launch.py`: `main`Executableを`device`namespace下で実行
+- `channel_launch.py`: `channel`Executableを`device`namespace下で実行
 - `receiver_launch.py`: `receiver`Executableを`device`namespace下で実行
 - `sender_launch.py`: `sender`Executableを`device`namespace下で実行

device/nucleo_communicate_py/launch/main_launch.py → device/nucleo_communicate_py/launch/channel_launch.py

@@ -5,7 +5,7 @@
 def generate_launch_description() -> LaunchDescription:
  main = Node(
  package="nucleo_communicate_py",
- executable="main",
+ executable="channel",
  namespace="device"
  )
  return LaunchDescription([main])

device/nucleo_communicate_py/nucleo_communicate_py/channel.py

@@ -0,0 +1,75 @@
+# ref:
+# https://github.com/ros2/examples/blob/7e47aee/rclpy/executors/examples_rclpy_executors/composed.py
+
+import sys
+
+import rclpy
+from packet_interfaces.msg import Current, Flex, Voltage, Power
+from rclpy.node import Node
+from std_msgs.msg import Empty
+
+from .mutex_serial import MutexSerial
+from .receiver import Recv
+from .sender import Sndr
+
+
+class Channel(Node):
+ def __init__(self, mutex_serial: MutexSerial):
+ super().__init__("channel")
+ # Senrder.__init__
+ self._quit_subscription = self.create_subscription(
+ Empty,
+ "quit",
+ self._quit_callback,
+ 10
+ )
+ self._order_subscription = self.create_subscription(
+ Power,
+ "order/power",
+ self._order_callback,
+ 10
+ )
+ self._sender = Sndr(mutex_serial)
+ # Receiver.__init__
+ 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._recv_callback)
+ self._recv = Recv(mutex_serial)
+
+ # Receiver implementation
+ def _recv_callback(self):
+ self.get_logger().debug("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(Current(value=current))
+ self._voltage_publisher.publish(Voltage(value=voltage))
+
+ # Sender implementations
+ def _quit_callback(self, _quit: Empty) -> None:
+ self._sender.send_quit()
+ self.get_logger().info("Sent quit 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)
+ # FIXME: receiver.pyのコメント参照
+ mutex_serial = MutexSerial("/dev/ttyACM0")
+ channel = Channel(mutex_serial)
+ rclpy.spin(channel)
+ channel.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()

device/nucleo_communicate_py/setup.py

@@ -3,7 +3,6 @@
 
 from setuptools import find_packages, setup
 
-
 package_name = 'nucleo_communicate_py'
 
 setup(
@@ -12,7 +11,7 @@
  packages=find_packages(exclude=['test']),
  data_files=[
  ('share/ament_index/resource_index/packages',
-  ['resource/' + package_name]),
+ ['resource/' + package_name]),
  ('share/' + package_name, ['package.xml']),
  (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*')))
  ],
@@ -27,7 +26,7 @@
  'console_scripts': [
  "sender = nucleo_communicate_py.sender:main",
  "receiver = nucleo_communicate_py.receiver:main",
- "main = nucleo_communicate_py.main:main"
+ "channel = nucleo_communicate_py.channel:main"
  ],
  },
 )