LoginSignup
16
15

More than 3 years have passed since last update.

@nasu_onigiri

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

Last updated at Posted at 2019-12-08

はじめに

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

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

使用機材

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

（イメージ）
IMG_7437.JPG

作成するプログラム

大体のロボットは
「センサなどの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しつづけるだけのノードを作ります。
ノードの再利用性を高めるため、複雑な処理等はこのノードでは行いません。
ただセンサ値を流しつづけることに専念します。

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つのサーボに割り当てます。
（この使い方が正しいのかはわかりませんが...。）

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 に変化したときに右側のサーボを適当に動かすノードを作成します（全く知的じゃない）。

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 ファイルを読み込めるようにします。

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
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() の出力が表示されませんでした。
このスレッドによると、 "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 を用いた最低限のロボットプログラムを作成しました。
最終的な成果物は、こちらにも置いておきます。
ROS2 における Best Practice 的なものがよくわかっていないので、今後も要勉強です。

16
15
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
16
15