From e36f0dbc56d1181d2fa14bf19b46f44da8bb9535 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 29 May 2024 23:26:09 +0900 Subject: [PATCH] Add nucleo_communicate_py package --- device/nucleo_communicate_py/README.md | 20 ++++++++ .../nucleo_communicate_py/__init__.py | 0 .../nucleo_communicate_py/main.py | 30 ++++++++++++ .../nucleo_communicate_py/receiver.py | 34 ++++++++++++++ .../nucleo_communicate_py/sender.py | 47 +++++++++++++++++++ device/nucleo_communicate_py/package.xml | 17 +++++++ .../resource/nucleo_communicate_py | 0 device/nucleo_communicate_py/setup.cfg | 4 ++ device/nucleo_communicate_py/setup.py | 29 ++++++++++++ device/nucleo_communicate_py/test/.gitkeep | 0 10 files changed, 181 insertions(+) create mode 100644 device/nucleo_communicate_py/README.md create mode 100644 device/nucleo_communicate_py/nucleo_communicate_py/__init__.py create mode 100644 device/nucleo_communicate_py/nucleo_communicate_py/main.py create mode 100644 device/nucleo_communicate_py/nucleo_communicate_py/receiver.py create mode 100644 device/nucleo_communicate_py/nucleo_communicate_py/sender.py create mode 100644 device/nucleo_communicate_py/package.xml create mode 100644 device/nucleo_communicate_py/resource/nucleo_communicate_py create mode 100644 device/nucleo_communicate_py/setup.cfg create mode 100644 device/nucleo_communicate_py/setup.py create mode 100644 device/nucleo_communicate_py/test/.gitkeep diff --git a/device/nucleo_communicate_py/README.md b/device/nucleo_communicate_py/README.md new file mode 100644 index 0000000..5a339d6 --- /dev/null +++ b/device/nucleo_communicate_py/README.md @@ -0,0 +1,20 @@ +# nucleo communication + +Nucleoとの通信周り + +TODO: いつかC++に書き換える + +## Nodes + +- `Receiver`: Nucleoからデータを受け取る +- `Sender`: Nucleoにデータを送る + +## Executables + +- `receiver`: `Receiver`Nodeをspin +- `sender`: `Sender`Nodeをspin +- `main`: `Receiver`, `Sender`Node2つを同時にspin + +## Launches + +TODO diff --git a/device/nucleo_communicate_py/nucleo_communicate_py/__init__.py b/device/nucleo_communicate_py/nucleo_communicate_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/device/nucleo_communicate_py/nucleo_communicate_py/main.py b/device/nucleo_communicate_py/nucleo_communicate_py/main.py new file mode 100644 index 0000000..4124215 --- /dev/null +++ b/device/nucleo_communicate_py/nucleo_communicate_py/main.py @@ -0,0 +1,30 @@ +# ref: +# https://github.com/ros2/examples/blob/7e47aee/rclpy/executors/examples_rclpy_executors/composed.py + +import sys + +import rclpy +from rclpy.executors import SingleThreadedExecutor, ExternalShutdownException + +from .receiver import Receiver +from .sender import Sender + + +def main(args=sys.argv): + rclpy.init(args=args) + try: + sender = Sender() + receiver = Receiver() + + executor = SingleThreadedExecutor() + executor.add_node(sender) + executor.add_node(receiver) + executor.spin() + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/device/nucleo_communicate_py/nucleo_communicate_py/receiver.py b/device/nucleo_communicate_py/nucleo_communicate_py/receiver.py new file mode 100644 index 0000000..08eeffe --- /dev/null +++ b/device/nucleo_communicate_py/nucleo_communicate_py/receiver.py @@ -0,0 +1,34 @@ +import sys + +import rclpy +from rclpy.node import Node +from packet_interfaces.msg import Current, Flex, Voltage + + +class Receiver(Node): + def __init__(self) -> 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) + + def _timer_callback(self) -> None: + # TODO + # https://github.com/rogy-AquaLab/2024_cavolinia_nucleo 参照 + # UARTでNucleoからデータを取得してpublishする + self.get_logger().info("tick") + + +def main(args=sys.argv): + rclpy.init(args=args) + receiver = Receiver() + rclpy.spin(receiver) + receiver.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/device/nucleo_communicate_py/nucleo_communicate_py/sender.py b/device/nucleo_communicate_py/nucleo_communicate_py/sender.py new file mode 100644 index 0000000..c329a64 --- /dev/null +++ b/device/nucleo_communicate_py/nucleo_communicate_py/sender.py @@ -0,0 +1,47 @@ +import sys + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Empty +from packet_interfaces.msg import Power + + +class Sender(Node): + def __init__(self) -> None: + super().__init__("sender") + 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 + ) + + 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") + + 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 main(args=sys.argv): + rclpy.init(args=args) + sender = Sender() + rclpy.spin(sender) + sender.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/device/nucleo_communicate_py/package.xml b/device/nucleo_communicate_py/package.xml new file mode 100644 index 0000000..a6c5d71 --- /dev/null +++ b/device/nucleo_communicate_py/package.xml @@ -0,0 +1,17 @@ + + + + nucleo_communicate_py + 0.1.0 + Nucleoとの通信周り + h1rono + MIT + + rclpy + std_msgs + packet_interfaces + + + ament_python + + diff --git a/device/nucleo_communicate_py/resource/nucleo_communicate_py b/device/nucleo_communicate_py/resource/nucleo_communicate_py new file mode 100644 index 0000000..e69de29 diff --git a/device/nucleo_communicate_py/setup.cfg b/device/nucleo_communicate_py/setup.cfg new file mode 100644 index 0000000..20c9365 --- /dev/null +++ b/device/nucleo_communicate_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/nucleo_communicate_py +[install] +install_scripts=$base/lib/nucleo_communicate_py diff --git a/device/nucleo_communicate_py/setup.py b/device/nucleo_communicate_py/setup.py new file mode 100644 index 0000000..bbda243 --- /dev/null +++ b/device/nucleo_communicate_py/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup + + +package_name = 'nucleo_communicate_py' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='h1rono', + maintainer_email='hronok66@gmail.com', + description='Nucleoとの通信周り', + license='MIT', + tests_require=[], + entry_points={ + 'console_scripts': [ + "sender = nucleo_communicate_py.sender:main", + "receiver = nucleo_communicate_py.receiver:main", + "main = nucleo_communicate_py.main:main" + ], + }, +) diff --git a/device/nucleo_communicate_py/test/.gitkeep b/device/nucleo_communicate_py/test/.gitkeep new file mode 100644 index 0000000..e69de29