rclpyでROS2のserviceの挙動と独自srvを書いてみる

serviceはtopicと違い送受信に確認がされるので確実に伝えたい情報に使います
自作serviceの事件のため、まずはRaspberry PI 4BのGPIOをオンオフするROS serviceをpython3で実装してみます

実際のところ

必要なパッケージのインストール:

Raspberry Pi上でGPIOを操作するために、RPi.GPIOをインストールします。

pip3 install RPi.GPIO

ROS2パッケージの作成

インターフェイスとスクリプトのをそれぞれ

$ cd dev_ws/src
$ ros2 pkg create --build-type ament_cmake gpio_service_msg
$ ros2 pkg create gpio_service --build-type ament_python

インターフェイスの用意

serviceファイルなのでsrv。
どこかの莫迦のようにsrcと書いて詰まないようにしてください……。

$ cd ~/dev_ws/src
$ cd gpio_service_msg
$ mkdir srv
$ touch srv/GPIOControl.srv
$ ls
CMakeLists.txt  include  package.xml  src  srv

GPIOControl.srvを以下のように記述

$ cat GPIOControl.srv
int8 pin
bool state
---
bool success
string message

CMakeLists.txtのament_package()より前に

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/AddThreeInts.srv"
)

package.xmlの内の任意のところに

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

Pythonスクリプトの実装:

次にserviceを実行するスクリプトの方を作っていきます

$ cd ~/dev_ws/src
$ mkdir -p gpio_service_ctrl/gpio_service_ctrl
$ touch gpio_service_ctrl/gpio_service_ctrl/gpio_server.py

gpio_serve_ctrlに以下の内容を追加します。

import rclpy
from rclpy.node import Node
from gpio_service_msg.srv import GPIOControl
import RPi.GPIO as GPIO

class GPIOServer(Node):
    def __init__(self):
        super().__init__('gpio_server')
        self.srv = self.create_service(GPIOControl, 'set_gpio', self.set_gpio_callback)
        GPIO.setmode(GPIO.BCM)

    def set_gpio_callback(self, request, response):
        try:
            GPIO.setup(request.pin, GPIO.OUT)
            GPIO.output(request.pin, request.state)
            response.success = True
            response.message = "GPIO set successfully"
        except Exception as e:
            response.success = False
            response.message = str(e)
        return response

def main(args=None):
    rclpy.init(args=args)
    node = GPIOServer()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

実行可能スクリプトとして設定:

setup.pyの#21~#23行目で以下のようになっている所を

#...21行
      entry_points={
          'console_scripts': [
          ],
#...

以下のように編集します。

#...
entry_points={
    'console_scripts': [
        'gpio_server = gpio_service_ctrl.gpio_server:main'
    ],
},
#...

ビルド:

serviceとして使用できるようにビルドします
わざわざ全部やる事でもないので、パッケージは明示的に

$ cd ~/dev_ws
$ colcon build --packages-select gpio_service_ctrl gpio_service_msg

環境に反映

$ source install/setup.bash

interfaceが認識されている事の確認

$ ros2 interface show gpio_service_msg/srv/GPIOControl
int8 pin
bool state
---
bool success
string message

サービスの実行

$ ros2 run gpio_service_ctrl gpio_server

$ ros2 service call /set_gpio gpio_service_msg/srv/GPIOControl "{ pin: 23, state: true }"

これで、新しく作成されたサービスset_gpioを呼び出してGPIOをオンオフできるようになりました。

参考もと

Developing a ROS 2 package — ROS 2 Documentation: Rolling documentation
ament_cmake_python user documentation — ROS 2 Documentation: Rolling documentation
Creating custom msg and srv files — ROS 2 Documentation: Humble documentation