Merge pull request #50 from rogy-AquaLab/raspi_led

create_node_pi_led

device/pi_led/config/default_param.yml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+  #right
+  led_pin_r: "GPIO17"
+  led_pin_g: "GPIO27"
+  led_pin_b: "GPIO22"

device/pi_led/config/left_param.yml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+  led_pin_r: "GPIO10"
+  led_pin_g: "GPIO9"
+  led_pin_b: "GPIO11"

device/pi_led/launch/led_launch.py

@@ -0,0 +1,20 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    config = os.path.join(
+        get_package_share_directory("pi_led"),
+        "config",
+        "default_param.yml",
+    )
+    led = Node(
+        package="pi_led",
+        excutable="pi_led",
+        namespace="device",
+        parameters=[config],
+    )
+    return LaunchDescription([led])

device/pi_led/package.xml

@@ -0,0 +1,18 @@
+<?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_led</name>
+  <version>0.0.0</version>
+  <description>LEDを光らせる</description>
+  <maintainer email="[email protected]">23-saho</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>packet_interfaces</exec_depend>
+  <exec_depend>python3-gpiozero</exec_depend>
+
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

device/pi_led/pi_led/pi_led.py

@@ -0,0 +1,61 @@
+import rclpy
+from gpiozero import LED
+from packet_interfaces.msg import LedColor
+from rclpy.node import Node
+
+
+class Led(Node):
+    def __init__(self):
+        super().__init__("led")
+        self._led_subscription = self.create_subscription(
+            LedColor, "led_color", self.led_callback, 10
+        )
+        param_r = self.declare_parameter("led_pin_r", "GPIO17")
+        param_g = self.declare_parameter("led_pin_g", "GPIO27")
+        param_b = self.declare_parameter("led_pin_b", "GPIO22")
+
+        led_pin_r = (
+            self.get_parameter_or(param_r.name, param_r)
+            .get_parameter_value()
+            .string_value
+        )
+        led_pin_g = (
+            self.get_parameter_or(param_g.name, param_g)
+            .get_parameter_value()
+            .string_value
+        )
+        led_pin_b = (
+            self.get_parameter_or(param_b.name, param_b)
+            .get_parameter_value()
+            .string_value
+        )
+
+        self.led_r = LED(led_pin_r)
+        self.led_g = LED(led_pin_g)
+        self.led_b = LED(led_pin_b)
+
+    def led_callback(self, light: LedColor):
+        self.led_light(self.led_r, light.red)
+        self.led_light(self.led_g, light.green)
+        self.led_light(self.led_b, light.blue)
+        self.get_logger().info(
+            f'R is "{int(light.red)}",G is "{int(light.green)}",B is "{int(light.blue)}"'
+        )
+
+    def led_light(self, led, light):
+        if light:
+            led.on()
+        else:
+            led.off()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    led_subscriber = Led()
+    rclpy.spin(led_subscriber)
+    led_subscriber.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

device/pi_led/setup.cfg

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

device/pi_led/setup.py

@@ -0,0 +1,35 @@
+import os
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "pi_led"
+
+setup(
+    name=package_name,
+    version="0.0.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", "*")),
+        ),
+        (
+            os.path.join("share", package_name, "config"),
+            glob(os.path.join("config", "*")),
+        ),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="23-saho",
+    maintainer_email="[email protected]",
+    description="LEDを光らせる",
+    license="MIT",
+    entry_points={
+        "console_scripts": [
+            "pi_led = pi_led.py_led:main",
+        ],
+    },
+)