Raspberry Pi + ROS2 で、最低限のロボットプログラム（Python）を作成

# はじめに

Raspberry Pi で ROS2 + I2C + GPIO が使える環境で、最低限のロボットプログラムをPythonで作ります。
Raspberry Pi 4 を使う場合は、[前回の記事](https://qiita.com/nasu_onigiri/items/39a0b16107e8e7fbbbe1)を参照。
サーボやセンサ周りの接続などは、他記事に任せます。

「ハード使わなくていいから、とりあえずソフトだけ動かしたい」という人は、ハードの部分を適当なプログラムで置き換えてください。



## 使用機材
- Raspberry Pi 4
- SDカード & SDカード書き込み用PC
- 人感センサ... HC-SR501
- サーボモータ*2つ... MG90S
- サーボドライバ... PCA9685
- 電池ボックス & 単3電池*4本
- ジャンパ線（メス-メス）*N本

（イメージ）
![IMG_7437.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/88538/1277fd33-9f64-5412-7c63-a1d9454cb35f.jpeg)




## 作成するプログラム
大体のロボットは
「センサなどのinputをもとに → 行動を決定して → 動いたり喋ったりのoutputを行う」
という制御がなされていると思います。

今回はこの各ステップをノード化して、3つのプログラムを作成します。

- input: センサ値を読み取って、publishしつづける（だけ）
- controller: ↑をsubscribeし、サーボ動作を決定してpublishする
- output: ↑をsubscribeし、サーボを動かす（だけ）


# ROS2 で 自作Pythonパッケージ作成
今回は、Pythonでサクッと作りましょう。
パッケージ名は rpi_robot_py として、以下進めていきます。

```
$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws/src/

$ ros2 pkg create  --build-type ament_python rpi_robot_py
$ cd rpi_robot_py
```

--build-type ament_python をつけると、Python用パッケージの雛形が作成されます（便利！）。
こんな感じのディレクトリ構造になっているはずです。

```
├── package.xml
├── resource
│   └── rpi_robot_py
├── rpi_robot_py
│   ├── __init__.py
├── setup.cfg
├── setup.py
└── test
    ├── test_copyright.py
    ├── test_flake8.py
    └── test_pep257.py
```

rpi_robot_py/ の中に、自作Pythonプログラムを作っていきます。
（このディレクトリ名がパッケージ名と同じなのは、そういう慣習なのか...?）

# input（センサ）ノードの作成
0.5秒間隔で人感センサ（HC-SR501）の値をpublishしつづけるだけのノードを作ります。
ノードの再利用性を高めるため、複雑な処理等はこのノードでは行いません。
ただセンサ値を流しつづけることに専念します。

```python:rpi_robot_py/input_human_sensor.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool

import pigpio
import time

SENSOR_GPIO_PIN = 17 # 17番PINにセンサ接続
TIMER_INTERVAL = 0.5

class HumanSensor(Node):
    def __init__(self):
        super().__init__('human_sensor_node')
        self.init_sensor() # センサを使わない場合、不要
        self.pub_sensor = self.create_publisher(Bool, '/input/human_sensor', 10)
        self.timer = self.create_timer(TIMER_INTERVAL, self.sensor_timer_callback)
        self.prev_sensor_data = False


    def sensor_timer_callback(self):
        sensor_msg = Bool(data=self.get_sensor_data())
        self.pub_sensor.publish(sensor_msg)


    def init_sensor(self):
        self.pi = pigpio.pi()
        self.pi.set_mode(SENSOR_GPIO_PIN, pigpio.INPUT)
        self.pi.set_pull_up_down(SENSOR_GPIO_PIN, pigpio.PUD_UP)

    # センサを使わない場合、True/Falseを返す適当な関数に置きかえてください
    def get_sensor_data(self):
        if self.pi.read(SENSOR_GPIO_PIN) == 1:
            return True
        else:
            return False


def main(args=None):
    rclpy.init(args=args)
    node = HumanSensor()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
```


# output（サーボ）ノードの作成
PCA9685と2つのサーボを使って、サーボを動かすノードを作成します。
ここでも複雑な処理はせず、指示をsubscribeしてサーボを動かすことだけに専念します。

また、ROS1では target（実行単位）= node でしたが、ROS2では単一ターゲット内で複数ノードを動かせるとのこと。
せっかくなので、これも試してみようと思います。

サーボ用のノード（クラス）を作成し、それを2つインスタンス化して、2つのサーボに割り当てます。
（この使い方が正しいのかはわかりませんが...。）

```python:rpi_robot_py/output_servo.py
import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor
# from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import Int8

import Adafruit_PCA9685

ANGLE_MIN = -70
ANGLE_MAX = 70

# PCA9685 の 0 と 3 に、2つのサーボを接続
SERVO_LEFT_ID = 0
SERVO_RIGHT_ID = 3

class Servo(Node):
    def __init__(self, servo_name, servo_id):
        super().__init__('servo_node_' + servo_name)
        self.init_pca9685() # サーボを使わない場合、不要
        self.servo_name = servo_name
        self.servo_id = servo_id
        self.sub_topic_name = '/output/servo/' + servo_name
        self.sub_servo = self.create_subscription(Int8, self.sub_topic_name, self.servo_callback, 10)


    def init_pca9685(self):
        self.pwm = Adafruit_PCA9685.PCA9685(address=0x40)
        self.pwm.set_pwm_freq(60)


    def servo_callback(self, servo_msg):
        self.get_logger().info('subscribe servo angle: {}'.format(servo_msg.data))
        self.set_angle(servo_msg.data)

    # サーボを使わない場合、適当な関数に置きかえてください
    def set_angle(self, angle):
        angle = max(ANGLE_MIN, angle)
        angle = min(ANGLE_MAX, angle)
        pulse = (600-150) / 180 * (angle + 90) + 150
        self.pwm.set_pwm(self.servo_id, 0, int(pulse))


def main(args=None):
    rclpy.init(args=args)

    executor = SingleThreadedExecutor()
    ## mutli thread の場合はこっち
    # executor = MultiThreadedExecutor(num_threads=2)

    ## ノード（クラス）を2つインスタンス化し、executor に登録
    node_left = Servo(servo_name='left', servo_id=SERVO_LEFT_ID)
    node_right = Servo(servo_name='right', servo_id=SERVO_RIGHT_ID)
    executor.add_node(node_left)
    executor.add_node(node_right)

    try:
        executor.spin()
    except KeyboardInterrupt:
        pass

    executor.shutdown()
    node_left.destroy_node()
    node_right.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
```

Int8型のメッセージで角度を指定し、/output/servo/left か /output/servo/right でpublishすると、サーボが動くプログラムが完成しました。


# Controllerノードの作成
センサ値を流し続けてくれるノードと、角度を受け取ってサーボを動かすノードが用意できました。
最後に、そのセンサ値を受け取ってサーボの動かし方を決定する、いわゆる知的処理をするノードを作成しましょう。

今回は人感センサの値が False->True に変化したときに左側のサーボを、True->False に変化したときに右側のサーボを適当に動かすノードを作成します（全く知的じゃない）。

```python:rpi_robot_py/controller.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from std_msgs.msg import Int8

import random

ANGLE_MIN = -70
ANGLE_MAX = 70

class MyController(Node):
    def __init__(self):
        super().__init__('controller_node')
        self.prev_sensor_data = False
        self.pub_servo_left = self.create_publisher(Int8, '/output/servo/left', 10)
        self.pub_servo_right = self.create_publisher(Int8, '/output/servo/right', 10)
        self.sub_sensor = self.create_subscription(Bool, '/input/human_sensor', self.sensor_callback, 10)


    def sensor_callback(self, sensor_msg):
        if self.prev_sensor_data != sensor_msg.data:
            self.prev_sensor_data = sensor_msg.data
            if sensor_msg.data == True:
                self.pub_servo_left.publish( Int8(data=random.randint(ANGLE_MIN, ANGLE_MAX)) )
            else: 
                self.pub_servo_right.publish( Int8(data=random.randint(ANGLE_MIN, ANGLE_MAX)) )


def main(args=None):
    rclpy.init(args=args)
    node = MyController()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
```

# 実行の準備

パッケージ作成時に自動生成された setup.py に、今回作成した3つのプログラムの情報を追記。
加えて、launch/ に後で作成する *.launch.py ファイルを読み込めるようにします。

```python:setup.py
from glob import glob
from setuptools import setup

package_name = 'rpi_robot_py'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ## 後で作成する 'launch/*.launch.py'用に追記
        ('share/' + package_name, glob('launch/*.launch.py'))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ns',
    maintainer_email='ns@hoge.com',
    description='sample robot program',
    license='',
    tests_require=['pytest'],
    ## 追記
    entry_points={
        'console_scripts': [
            'human_sensor = rpi_robot_py.input_human_sensor:main',
            'servo = rpi_robot_py.output_servo:main',
            'controller = rpi_robot_py.controller:main',
        ],
    },
)
```

3つのプログラムを一つ一つ起動するのは面倒なので、まとめて起動するlaunchファイルを用意します。
細かいことは気にせず、とにかく3つのプログラムを実行するlaunchファイル。

```
$ cd ~/ros2_ws/src/rpi_robot_py
$ mkdir launch
$ cd launch
$ touch rpi_robot.launch.py
```

```python:launch/rpi_robot.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='rpi_robot_py',
            node_executable='human_sensor',
        ),
        Node(
            package='rpi_robot_py',
            node_executable='servo',
            output='screen'
        ),
        Node(
            package='rpi_robot_py',
            node_executable='controller',
        )
    ])
```


build します。


```
$ cd ~/ros2_ws
$ colcon build
$ . install/setup.bash
```

最終的なディレクトリ構成はこのようになりました。

```
├── launch
│   └── rpi_robot.launch.py
├── package.xml
├── resource
│   └── rpi_robot_py
├── rpi_robot_py
│   ├── controller.py
│   ├── __init__.py
│   ├── input_human_sensor.py
│   └── output_servo.py
├── setup.cfg
├── setup.py
└── test
    ├── test_copyright.py
    ├── test_flake8.py
    └── test_pep257.py
```

最後にpigpioを使うための準備をして、完了です。

```
$ sudo pigpiod
```



# 実行
launchファイルを使って、3つのプログラムを全部起動します。

```
$ ros2 launch rpi_robot_py rpi_robot.launch.py

[INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2019-12-08-09-34-30-479378-ubuntu-31195
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [human_sensor-1]: process started with pid [31205]
[INFO] [servo-2]: process started with pid [31206]
[INFO] [controller-3]: process started with pid [31207]
```

無事に3つのプログラムが起動しました。

が、なぜか self.get_logger().info() の出力が表示されませんでした。
[このスレッド](https://answers.ros.org/question/332829/no-stdout-logging-output-in-ros2-using-launch/)によると、 "stdbuf -o L" を前につけるととりあえずは表示されるらしい。

```
$ stdbuf -o L ros2 launch rpi_robot_py rpi_robot.launch.py

[INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2019-12-08-09-41-45-243668-ubuntu-31234
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [human_sensor-1]: process started with pid [31244]
[INFO] [servo-2]: process started with pid [31245]
[INFO] [controller-3]: process started with pid [31246]
[servo-2] [INFO] [servo_node_left]: subscribe servo angle: 17
[servo-2] [INFO] [servo_node_right]: subscribe servo angle: 48
...
```

# おわりに
ROS2 を用いた最低限のロボットプログラムを作成しました。
最終的な成果物は、[こちら](https://github.com/namiky73/ros2-sample/tree/master/rpi_robot_py)にも置いておきます。
ROS2 における Best Practice 的なものがよくわかっていないので、今後も要勉強です。