ここでは「Ubuntu Server 20.04.2 LTS」と「ROS2 Foxy」をインストールした「Raspberry Pi 4 Model B」を使って、サーボモータを制御する方法を紹介します。
この記事を読むことで
ROS2でのpub、subノードの使い方
ROS2でのPWM制御の一例
を知ることができ
のようにサーボモータを制御することができます。
メッセージ内容の変更やGPIOピンを増やして、複数のサーボモータを制御することもできます。
ROS2でロボットやシステムを制御したい方は是非挑戦してみてください。
それでは早速始めていきましょう。
※ 当サイトのプログラムを改良したものを公開、配布する場合などは「引用元のURL、ブログ名、記事名」をご記入の上、使用してくださると私のモチベーションが上がります。(>_<)
ROS2でサーボモータを制御する手順
今回は、ROS2のPublisherとlistenerを用いて、サーボモータを制御していきます。
Raspberry Pi 4 Model BへUbuntuをインストールする方法や、ROS2の基本的な使い方は当サイトでも紹介しているので、合わせてご覧ください。
サーボモータ制御用のパッケージをインストール＆作成
まずはサーボモータを制御するためのパッケージをインストールします。
python-rpi.gpioのインストール
を実行し、rpi.gpio使えるようにしましょう。
インストールが終了したら
gpio -v
gpio readall
などでラズパイUbuntuでGPIOピンが使用できる状態かを確認します。
上図のように表示されていれば、問題ありません。
サーボモータ制御用ROS2パッケージの作成
続いて、サーボモータをROS2で制御するためのパッケージを作成します。
pub-subパッケージの作成の基礎は下記サイトでも紹介しているので、心配な方はこちらも合わせてご覧ください。
ターミナルを開いたら
でワークスペースのROS2パッケージ管理ディレクトリへ移動します。
※ ROS2 Foxyチュートリアルでは、srcフォルダにてパッケージを管理しています。
そのため、別の名前でパッケージ管理している方は、任意のPATHへ変更してください。
ディレクトリを移動したら
を実行し、pigpio_pubsubという名前のパッケージを作成します。
パッケージを作成したら実際にプログラムを作成＆記述していきましょう！
ROS2でサーボモータ制御用プログラムを作成する
現在のディレクトリは「cd ~/dev_ws/src」になっているので（上記通りなら）
を実行し、プログラムを管理しているフォルダへPATHを移動します。
（もちろん絶対パスを指定してもOK）
ls コマンドで中身を確認してみると、Pythonパッケージである事を示す「_init_.py」が入っているはずです。
このフォルダ内に、publisherとsubscriberのPythonファイルを作っていきます。
touch subscriber_gpio.py
を実行し、Pythonファイルを作成したら
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ですが、
で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制御するときの考え方と同じでして、当サイトでも下記記事にて紹介しているので、原理を知りたい方は合わせてご覧ください。
サーボモータのパルスや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関数内に、実際に動かしたい処理を記述するといったイメージで問題ないかと思います。
記述が終わったら
で、ディレクトリを移動し、セットファイルや.xmlファイルの設定を変更しましょう。
まずは
でpackage.xmlファイルの下記の部分を編集します。
package.xml
description：ros2_servo license：Apache License 2.0
descriptionやライセンス名は任意で変更可能です。
続いて、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ピンを使用することができるので、お好みにカスタマイズしてみてください。
サーボモータや電源供給用のバッテリーの準備ができたら
でディレクトリを移動し
とパッケージを選択して、ビルドします。
ビルドが終了したら
を実行し
でtalkerのノードを走らせます。
続いて、新しくターミナルを立ち上げ
cd ~/dev_ws
. install/setup.bash
ros2 run pigpio_pubsub listener
を順に実行しましょう。
すると
とROS2でサーボモータを制御することができます。
是非、ロボット制御などに導入してみてください！
お疲れ様でした。