Merge pull request #4 from rogy-AquaLab/pi-i2c

device/pi_i2c

device/pi_i2c/README.md

@@ -0,0 +1,20 @@
+# nucleo communication
+
+Raspberry PiのI2C接続
+
+## Nodes
+
+- `Depth`: 深さセンサー
+- `Imu`: IMU
+
+## Executables
+
+- `depth`: `Depth`Nodeをspin
+- `imu`: `Imu`Nodeをspin
+- `all`: `Depth`, `Imu`Node2つを同時にspin
+
+## Launches
+
+- `all_launch.py`: `all`executableを`device`namespace下で実行
+- `depth_launch.py`: `depth`executableを`device`namespace下で実行
+- `imu_launch.py`: `imu`executableを`device`namespace下で実行

device/pi_i2c/launch/all_launch.py

@@ -0,0 +1,11 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+ all_exec = Node(
+ package="pi_i2c",
+ executable="all",
+ namespace="device"
+ )
+ return LaunchDescription([all_exec])

device/pi_i2c/launch/depth_launch.py

@@ -0,0 +1,11 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+ depth = Node(
+ package="pi_i2c",
+ executable="depth",
+ namespace="device"
+ )
+ return LaunchDescription([depth])

device/pi_i2c/launch/imu_launch.py

@@ -0,0 +1,11 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+ imu = Node(
+ package="pi_i2c",
+ executable="imu",
+ namespace="device"
+ )
+ return LaunchDescription([imu])

device/pi_i2c/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>pi_i2c</name>
+ <version>0.1.0</version>
+ <description>Raspberry PiのI2C接続</description>
+ <maintainer email="[email protected]">h1rono</maintainer>
+ <license>MIT</license>
+
+ <exec_depend>rclpy</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
+ <exec_depend>packet_interfaces</exec_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>

device/pi_i2c/pi_i2c/all.py

@@ -0,0 +1,27 @@
+import sys
+
+import rclpy
+from rclpy.executors import SingleThreadedExecutor, ExternalShutdownException
+
+from .depth import Depth
+from .imu import Imu
+
+
+def main(args=sys.argv):
+ rclpy.init(args=args)
+ try:
+ depth = Depth()
+ imu = Imu()
+
+ executor = SingleThreadedExecutor()
+ executor.add_node(depth)
+ executor.add_node(imu)
+ executor.spin()
+ except KeyboardInterrupt:
+ pass
+ except ExternalShutdownException:
+ sys.exit(1)
+
+
+if __name__ == "__main__":
+ main()

device/pi_i2c/pi_i2c/depth.py

@@ -0,0 +1,29 @@
+import sys
+
+import rclpy
+from rclpy.node import Node
+from packet_interfaces.msg import Depth as DepthMsg
+
+
+class Depth(Node):
+ def __init__(self) -> None:
+ super().__init__("depth")
+ self._current_publisher = self.create_publisher(DepthMsg, "depth", 10)
+ self._timer = self.create_timer(0.5, self._timer_callback)
+
+ def _timer_callback(self) -> None:
+ # TODO
+ # 深さセンサーからデータを取得してpublish
+ self.get_logger().info("tick")
+
+
+def main(args=sys.argv):
+ rclpy.init(args=args)
+ depth = Depth()
+ rclpy.spin(depth)
+ depth.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()

device/pi_i2c/pi_i2c/imu.py

@@ -0,0 +1,29 @@
+import sys
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu as ImuMsg
+
+
+class Imu(Node):
+ def __init__(self) -> None:
+ super().__init__("imu")
+ self._current_publisher = self.create_publisher(ImuMsg, "imu", 10)
+ self._timer = self.create_timer(0.5, self._timer_callback)
+
+ def _timer_callback(self) -> None:
+ # TODO
+ # IMUからデータを取得してpublish
+ self.get_logger().info("tick")
+
+
+def main(args=sys.argv):
+ rclpy.init(args=args)
+ imu = Imu()
+ rclpy.spin(imu)
+ imu.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()

device/pi_i2c/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pi_i2c
+[install]
+install_scripts=$base/lib/pi_i2c

device/pi_i2c/setup.py

@@ -0,0 +1,33 @@
+import os
+from glob import glob
+
+from setuptools import find_packages, setup
+
+
+package_name = 'pi_i2c'
+
+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']),
+ (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*')))
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='h1rono',
+ maintainer_email='[email protected]',
+ description='Raspberry PiのI2C接続',
+ license='MIT',
+ tests_require=[],
+ entry_points={
+ 'console_scripts': [
+ "depth = pi_i2c.depth:main",
+ "imu = pi_i2c.imu:main",
+ "all = pi_i2c.all:main"
+ ],
+ },
+)