ROS2を使ってみる(3)

ロボット

前回の記事のoutputノードを動かしても、動作するトリガーがなにも設定されていないので動作しません。

そこでコマンドラインから擬似メッセージをパブリッシュしてみます。(このフォーマットがわからんかった!)

まず

ros2 topic list

とすると、/output/servo/left, /output/servo/rightなどが存在しているはずです。
次に

ros2 topic info /output/servo/left

と入力すると、/output/servo/leftがどういうメッセージの型なのか、パブリッシュされた回数などが出ます。
ここではTypeはstd_msgs/msg/Int8となっているはずです。
そこでデータをパブリッシュしてみます。

ros2 topic pub --once /output/servo/left std_msgs/msg/Int8 "data: 45"

と入力すると左用モーターが45度に回ります。–onceを入れていないと、同じメッセージがどんどんパブリッシュされてしまいます。
角度を変えてメッセージを送ってみてください。

これで、プログラムの動作が確認できました。

最後に人感センサーデータを受け取って、サーボモーターを動かすコントローラーノードを作ります。


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


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.in_sr501:main',
            'servo = rpi_robot_py.out_servo:main',
            'controller = rpi_robot_py.controller:main',
        ],
    },
)

Launchファイルを作って、3つのプログラムを自動的に起動できるようにします。

cd ~/ros2_1/src/rpi_robot_py
mkdir launch
cd launch
touch rpi_robot.launch.py

rpi_robot.lunch.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',
        )
    ])

 

現在、ソースファイルは以下のように置いてあります。(赤字は編集、追加したファイル)

src
└── rpi_robot_py
    ├── launch
    │   └── rpi_robot.launch.py
    ├── package.xml
    ├── resource
    │   └── rpi_robot_py
    ├── rpi_robot_py
    │   ├── controller.py
    │   ├── __init__.py
    │   ├── in_sr501.py
    │   └── out_servo.py
    ├── setup.cfg
    ├── setup.py
    └── test
        ├── test_copyright.py
        ├── test_flake8.py
        └── test_pep257.py


次の手順でbuildします。

cd ~/ros2_1
colcon build
bash ./install/setup.bash

colconの結果は以下のとおり。


ros2 launch rpi_robot_py rpi_robot.launch.py

で、動きだしました。角度がランダムなので動きは若干、不気味です。笑

定義などを間違えている時は、colcon buildはやりなおさねばなりません。

半分くらい意味がわかっていませんが、ROS2で基本的な動きを知ることができました。記事を制作してくれた@nasu_onigiri氏には感謝です。