注目キーワード
  1. Photoshop
  2. Python
  3. Raspberry Pi
  4. Arduino

ROS2 FoxyとUbuntuラズパイでサーボモータを制御してみた

  • 2021年6月20日
  • 2021年7月18日
  • ROS2

ここでは「Ubuntu Server 20.04.2 LTS」と「ROS2 Foxy」をインストールした「Raspberry Pi 4 Model B」を使って、サーボモータを制御する方法を紹介します。

 

この記事を読むことで

ROS2でのpub、subノードの使い方

ROS2でのPWM制御の一例

を知ることができ

 

ROS2_servomovie

のようにサーボモータを制御することができます。

メッセージ内容の変更やGPIOピンを増やして、複数のサーボモータを制御することもできます。

 

ROS2でロボットやシステムを制御したい方は是非挑戦してみてください。

それでは早速始めていきましょう。

 

※ 当サイトのプログラムを改良したものを公開、配布する場合などは「引用元のURL、ブログ名、記事名」をご記入の上、使用してくださると私のモチベーションが上がります。(>_<)

 

ROS2でサーボモータを制御する手順

今回は、ROS2のPublisherとlistenerを用いて、サーボモータを制御していきます。

Raspberry Pi 4 Model BへUbuntuをインストールする方法や、ROS2の基本的な使い方は当サイトでも紹介しているので、合わせてご覧ください。

 

ラズパイ-Ubuntu20.04LTSの初期設定-SSH接続やIPアドレス固定など

 

サーボモータ制御用のパッケージをインストール&作成

まずはサーボモータを制御するためのパッケージをインストールします。

 

python-rpi.gpioのインストール

ROS2_servo_02

 

sudo apt-get install python-rpi.gpio

を実行し、rpi.gpio使えるようにしましょう。

 

インストールが終了したら

gpio -v

gpio readall

などでラズパイUbuntuでGPIOピンが使用できる状態かを確認します。

 

ROS2_servo_03

 

上図のように表示されていれば、問題ありません。

 

サーボモータ制御用ROS2パッケージの作成

続いて、サーボモータをROS2で制御するためのパッケージを作成します。

pub-subパッケージの作成の基礎は下記サイトでも紹介しているので、心配な方はこちらも合わせてご覧ください。

 

ROS2で単純なPublisher&Subscriberのノード間通信をPythonで作成・実行してみた

 

ターミナルを開いたら

cd ~/dev_ws/src

でワークスペースのROS2パッケージ管理ディレクトリへ移動します。

 

※ ROS2 Foxyチュートリアルでは、srcフォルダにてパッケージを管理しています。

そのため、別の名前でパッケージ管理している方は、任意のPATHへ変更してください。

 

ディレクトリを移動したら

ros2 pkg create –build-type ament_python pigpio_pubsub

を実行し、pigpio_pubsubという名前のパッケージを作成します。

 

パッケージを作成したら実際にプログラムを作成&記述していきましょう!

 

ROS2でサーボモータ制御用プログラムを作成する

現在のディレクトリは「cd ~/dev_ws/src」になっているので(上記通りなら)

cd ./pigpio_pubsub/pigpio_pubsub

を実行し、プログラムを管理しているフォルダへPATHを移動します。

(もちろん絶対パスを指定してもOK)

 

ls コマンドで中身を確認してみると、Pythonパッケージである事を示す「_init_.py」が入っているはずです。

 

このフォルダ内に、publisherとsubscriberのPythonファイルを作っていきます。

touch publisher_gpio.py
touch subscriber_gpio.py

を実行し、Pythonファイルを作成したら

sudo nano publisher_gpio.py
sudo nano subscriber_gpio.py

など好みの編集モードでPythonファイルにプログラムを記述していきます。

 

 publisher_gpio.py

import rclpy
from rclpy.node import Node

from std_msgs.msg import Float64


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(Float64, 'topic',10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

        self.angle_value = [-45, -30, -15, 0, 15, 30, 45, 30, 15, 0, -15, -30]
        self.num = 0

    def timer_callback(self):
        msg = Float64()
        msg.data = float(self.angle_value[self.num])
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing Servo angle: "%s"' % msg.data)

        self.num += 1
        if self.num == len(self.angle_value):
            self.num = 0
       

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

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

 

publisher_gpio.py は 、self.angle_valueで与えた角度データをmsg.dataでsubscriberへ送信します。

送信の度に配列番号を1ずつズラしているため、配列内の角度データを順に送ることができるようになっています。

 

main文は、publisher関数の実行とループ処理をしています。

 

pub-subチュートリアルを理解していれば、さほど難しくはないかと思います。

 

subscriber_gpio.py

import RPi.GPIO as GPIO
import rclpy
from rclpy.node import Node

from std_msgs.msg import Float64


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            Float64,
            'topic',
            self.listener_callback,
            30)
        self.subscription  # prevent unused variable warning

        self.servo_pin = [5, 6, 13, 19, 26]
        self.servo = []
        self.periodic_time = 22
        self.frec = round( 1/self.periodic_time * 1000 ,1)
        self.neutral = 1500/1000
        self.variable = 600/1000

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)

        for num in range(len(self.servo_pin)):
            GPIO.setup(self.servo_pin[num], GPIO.OUT)
            self.servo.append(GPIO.PWM(self.servo_pin[num], self.frec))
            self.servo[num].start(0)

    def servo_angle(self, angle, pin):
        duty = \
        ((self.neutral+(angle*self.variable/90))/self.periodic_time )*100

        self.servo[pin].ChangeDutyCycle(duty)

    def listener_callback(self, msg):
        self.get_logger().info('Change Servo angle: "%s"' % msg.data)
        self.servo_angle(float(msg.data), 0)  # GPIO 5 pin
        self.servo_angle(float(msg.data), 1)  # GPIO 6 pin
        self.servo_angle(float(msg.data), 2)  # GPIO 13 pin
        self.servo_angle(float(msg.data), 3)  # GPIO 19 pin
        self.servo_angle(float(msg.data), 4)  # GPIO 26 pin

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

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

 

続いて、subscriber_gpio.pyですが、

self.servo_pin = [5, 6, 13, 19, 26]

でGPIOの5、6、13、19、26番ピンでサーボモータ制御ができるように宣言しています。

 

self.servo = []
self.periodic_time = 22
self.frec = round( 1/self.periodic_time * 1000 ,1)
self.neutral = 1500/1000
self.variable = 600/1000

は、使用するサーボモータの駆動パルスや周波数、可変範囲を設定しています。

 

この部分はラズパイOSでGPIO制御するときの考え方と同じでして、当サイトでも下記記事にて紹介しているので、原理を知りたい方は合わせてご覧ください。

 

ラズパイで複数サーボをPythonで制御する~各社サーボ対応

 

サーボモータのパルスやDuty比の設定をしたら

def listener_callback(self, msg):
    self.get_logger().info('Change Servo angle: "%s"' % msg.data)
    self.servo_angle(float(msg.data), 0)

とcallback関数で、msg.dataを受信し、publisherから受信した角度データをservo_angle関数へ与えて制御します。

 

callback関数内に、実際に動かしたい処理を記述するといったイメージで問題ないかと思います。

 

記述が終わったら

cd ~/dev_ws/src/pigpio_pubsub

で、ディレクトリを移動し、セットファイルや.xmlファイルの設定を変更しましょう。

 

まずは

sudo nano package.xml

でpackage.xmlファイルの下記の部分を編集します。

 

package.xml

description:ros2_servo
license:Apache License 2.0

 

descriptionやライセンス名は任意で変更可能です。

 

続いて、setup.pyも

sudo nano setup.py

で編集していきます。

 

setup.py

description:ros2_servo
license:Apache License 2.0

entry_points={
    'console_scripts':[
        'talker = pigpio_pubsub.publisher_gpio:main',
        'listener = pigpio_pubsub.subscriber_gpio:main',
    ],
},

 

entry_pointsはインデントを間違えると

エラーメッセージが発生せずに、プログラムだけ動かない

といった状態になるので、気をつけてください。

 

これらの設定が終了したら、さっそくビルドして実行してみましょう。

 

実際にサーボモータをROS2で動かしてみる

サーボモータは上記で記述した5、6、13、19、23ピンのどれかに信号線を繋げてもらえばOKです。

また、servo_pinのピン番号を変えれば、任意のGPIOピンを使用することができるので、お好みにカスタマイズしてみてください。

 

サーボモータや電源供給用のバッテリーの準備ができたら

cd ~/dev_ws

でディレクトリを移動し

colcon build –packages-select pigpio_pubsub

とパッケージを選択して、ビルドします。

 

ビルドが終了したら

. install/setup.bash

を実行し

ros2 run pigpio_pubsub talker

でtalkerのノードを走らせます。

 

続いて、新しくターミナルを立ち上げ

cd ~/dev_ws

. install/setup.bash

ros2 run pigpio_pubsub listener

を順に実行しましょう。

 

すると

ROS2_servo_01

とROS2でサーボモータを制御することができます。

 

是非、ロボット制御などに導入してみてください!

お疲れ様でした。

ROS2_servo_topthumbnail
学びに関する情報をチェック!