rclpyでRaspberry Pi 4BのCPU温度を積算するActionを実装する

動作状態や途中介入を実現できるActionもやってみる事にしました
簡単化のため、CPU温度を積み上げる様式にします

実際のところ

actionのビルド

$ cd ~/dev_ws
$ ros2 pkg create --build-type ament_cmake cpu_temp_action_msgs
$ mkdir cpu_temp_action_msgs/action
$ touch cpu_temp_action_msgs/action/CpuTemp.action.action

CMakeLists.txtのament_package()の上に

# action fileの追加
rosidl_generate_interfaces(${PROJECT_NAME}
  "action/CpuTemp.action"
)
# ↓これより上
ament_package()

package.xmlのtest dependのした辺りに

  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

スクリプト

$ ~/dev_ws/src
$ ros2 pkg create cpu_temp_action_server --build-type ament_python  --dependencies rclpy cpu_temp_action_msgs
$ touch src/cpu_temp_action_server/cpu_temp_action_server/server.py

server.pyを以下のように追記
feedbackは無くても実装はできるみたいですが……今回は練習なので

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from cpu_temp_action_msgs.action import CpuTemp

class CpuTempActionServer(Node):

    def __init__(self):
        super().__init__('cpu_temp_action_server')
        self._action_server = ActionServer(
            self,
            CpuTemp,
            'cpu_temperature',
            self.execute_callback)

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')
        total = 0.0

        feedback_msg = CpuTemp.Feedback()

        while total <= 300:  # totalが300を超えるまでのループ
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('Goal canceled')
                return CpuTemp.Result()

            temperature = self.get_cpu_temperature()
            feedback_msg.current_temperature = temperature
            total += temperature
            goal_handle.publish_feedback(feedback_msg)
            rclpy.sleep(1.0)

        goal_handle.succeed()

        result = CpuTemp.Result()
        result.total = total
        return result

    def get_cpu_temperature(self):
        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)
    action_server = CpuTempActionServer()

    rclpy.spin(action_server)
    action_server.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

setup.pyにもconsole_scriptのところに

        'console_scripts': [
           'cpu_server = cpu_temp_action_server.server:main'
        ],

ビルド

$ cd ~/dev_ws/
$ colcon build
$ source install/setup.bash

コマンド

$ ros2 run cpu_temp_action_server cpu_server

試しにアクションを実行してみると……

$ ros2 action send_goal /cpu_temperature cpu_temp_action_msgs/action/CpuTemp "{temperature: 0.0}" --feedback

Waiting for an action server to become available...
Sending goal:
     temperature: 0.0

Goal accepted with ID: db87eb9cce934e77b92e440eea8c2576

Feedback:
    current_temperature: 43.816

Feedback:
    current_temperature: 85.197

Feedback:
    current_temperature: 127.065

Feedback:
    current_temperature: 167.959

Feedback:
    current_temperature: 209.34

Feedback:
    current_temperature: 250.234

Feedback:
    current_temperature: 291.128

Feedback:
    current_temperature: 332.022

Result:
    total: 332.022

Goal finished with status: SUCCEEDED

参考もと