rclpyでROS2のtopicの挙動を標準実装のmsgをつかって書いてみる

前回の続きで、今度はtopicをやります
データの垂れ流しに向いた形式で、3D可視化ツールのrvizやシミュレーション環境のgazeboなんかで状態の取得に使われています
今回は実装のお試しということで、Raspberry Pi 4BのCPU温度の積算を通知するようにしてみます

実際のところ

新しいパッケージの用意

$ cd ~/ros2_ws/src
$ ros2 pkg create --build-type ament_python rpi_cpu_temp --dependencies rclpy std_msgs

パッケージのセットアップ

今回はパラメーターが単純なので、データ構造は標準装備のstd_msgsをつかうため、前回のようにビルドは不要です

ノードの実装

$ mkdir rpi_cpu_temp/rpi_cpu_temp
$ touch rpi_cpu_temp/rpi_cpu_temp/cpu_temp_publisher.py

cpu_temp_publisher.pyを以下のように実装

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64

class CpuTempPublisher(Node):

    def __init__(self):
        super().__init__('cpu_temp_publisher')
        self.publisher_ = self.create_publisher(Float64, 'cpu_temperature', 10)
        timer_period = 1.0  # 1 second
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        temp = self.get_cpu_temperature()
        msg = Float64()
        msg.data = temp
        self.publisher_.publish(msg)
        self.get_logger().info(f"Published CPU Temp: {msg.data}")

    @staticmethod
    def get_cpu_temperature():
        with open("/sys/class/thermal/thermal_zone0/temp", "r") as f:
            temp = float(f.read()) / 1000.0
        return temp

def main(args=None):
    rclpy.init(args=args)
    cpu_temp_node = CpuTempPublisher()
    rclpy.spin(cpu_temp_node)
    cpu_temp_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

ビルドと実行

rpi_cpu_temp/setup.pyにエントリポイントを追加します。

...
entry_points={
    'console_scripts': [
        'cpu_temp_publisher = rpi_cpu_temp.cpu_temp_publisher:main'
    ],
},
...

ビルド

$ cd dev_ws/
$ colcon build --packages-select rpi_cpu_temp
$ source install/setup.bash

実行

$ ros2 run rpi_cpu_temp cpu_temp_publisher

実行側の端末では、こんな調子でログがモリモリでてきます

[INFO] [1698484578.358275492] [cpu_temp_publisher]: Published CPU Temp: 39.92
[INFO] [1698484579.293513953] [cpu_temp_publisher]: Published CPU Temp: 38.946
[INFO] [1698484580.292640364] [cpu_temp_publisher]: Published CPU Temp: 40.407
[INFO] [1698484581.295173259] [cpu_temp_publisher]: Published CPU Temp: 39.433
...

これをtopicとして呼んであげると……

$ ros2 topic echo /cpu_temperature
data: 40.894
---
data: 40.894
---
data: 39.433
---
data: 40.407
---
data: 39.92
---
...

参考もと

rcl_interfaces/builtin_interfaces at rolling · ros2/rcl_interfaces · GitHub