ROSで動かすロボットカー作り(前進・後退だけできるようになった編)
2021年ももう終わりですね。
年末年始は何かしらものを作ってみたくなるものです(え？そんなことない…？)
というわけで今年の年末年始は今までやろうとしてやれなかったこととして、Raspberry Piを使ったROS対応ロボットづくりをスタートすることにしました。
まずはロボットのメインである車輪を動かすところをやってみたのでその記録です。
大晦日はここまでやってその記録をブログにまとめるところまでで終わるでしょうｗ。
今回作ったコード一式は以下のGithubのレポジトリのbacklog/step1ブランチで公開しています。
用意するもの
- Raspberry Pi 4(ロボットに載せられて、無線通信ができるやつならなんでもOK)
- ロボットの車体(なんでもいいですが、今回はこちらを使います)
- モータードライバーHAT
今回は用意するものもそこまで多くないので以下のWikiを参考にモーターを配線したらあとはよしなに組み立てて貰えばOKです(手抜きですみません…)
ワークスペースの作成
手順が長いので、以前のブログをご覧ください。
ていうかRaspberry Pi×ROSのネタ、これ以降何も進んでませんでしたねw
インストール
ワークスペースができたところで、必要なライブラリ、モジュール諸々インストールします。
モータードライバー
まずは今回使用するモータードライバーに必要な依存関係をインストールします。
sudo pip3 install pillow sudo pip3 install numpy sudo apt-get install libopenjp2-7 sudo apt-get install libatlas-base-dev
あとはPythonライブラリもインストールします。
cd sudo apt-get update sudo apt-get install python3-pip sudo pip3 install RPi.GPIO sudo pip3 install smbus
このモータドライバはI2Cを使うのでRaspi ConfigでI2Cを忘れずに有効にしておきます。
ROS
次にROSの依存関係もインストールしておきます。
sudo pip3 install trollius rosdep rospkg rosinstall_generator rosinstall wstool vcstools catkin_pkg
モータードライバーのコードをダウンロード
次にモータードライバーの公式で配布しているコードをダウンロードします。
以下のコマンドでRapsberry Piにダウンロードします。
wget https://www.waveshare.com/w/upload/9/9a/Motor_Driver_HAT_Code.7z
7zipを解凍するためのソフトが存在しないので、以下のコマンドからダウンロードしておきます。
sudo apt install p7zip-full
ダウンロードしたコードを以下のコードで解凍します。
7z x Motor_Driver_HAT_Code.7z
解凍したディレクトリの中の Raspberry Pi/python にある PCA9685.py と PCA9685.pyc はこのあと作成するパッケージにコピーするので保存先だけ確認しておきます。
パッケージの作成
今回のロボットカー用のパッケージを作成します。
ワークスペースのあるディレクトリ(先程のワークスペース作成の通りに作っていれば~/ros_catkin_ws)に移動して、パッケージを作成していきます。(sensor_msgsは今回使いませんが、今後のために入れておきます。)
cd ~/ros_catkin_ws source devel/setup.bash cd src catkin_create_pkg my_rober rospy geometry_msgs sensor_msgs
いつもならここで catkin_makeでビルドするところですが、このままだと標準で提供されているメッセージのパッケージがなくて、エラーになります。
Raspberry PiのROSはフルでパッケージを入れないので、中には使いたいパッケージを自分で入れないといけないみたいです(この原因を特定するのにめちゃくちゃ時間がかかりました)
わざわざ入れるぐらいなら妥協して、あり物でメッセージ送信したいですがちゃんとROSで扱うメッセージに合わせたオブジェクトを用意してくれていることですし、使わないわけにはいきません。
というわけで公式で配布されているメッセージのパッケージをワークスペースにクローンしておきます。
cd ~/ros_catkin_ws/src git clone https://github.com/ros/common_msgs.git
たったこれだけで、ビルドの準備ができました。あとは、以下のコマンドでパッケージをビルドしましょう。
cd ~/ros_catkin_ws catkin_make
コードの用意
モータードライバーのコードを移動
先程ダウンロードしたモータードライバーのコードをパッケージのディレクトリに配置します。 モータドライバーのサンプルコードを活かす感じで使ったのでかなり無理やり感があると自覚してますｗ。
roscd my_rober/ mkdir -p scripts/MotorDriver cp dir/to/PCA9685.py scripts/MotorDriver cp dir/to/PCA9685.pyc scripts/MotorDriver
ロボット用のコード
続いてロボットを動かすためのコードを用意します。
以下のコードをrober.pyでscriptsディレクトリに保存します。
#!/usr/bin/python3 import time import rospy from rospy.exceptions import ROSInterruptException from geometry_msgs.msg import Twist from sensor_msgs.msg import Range from MotorDriver.PCA9685 import PCA9685 Dir = [ 'forward', 'backward', ] pwm = PCA9685(0x40, debug=False) pwm.setPWMFreq(50) class MotorDriver(): def __init__(self): self.PWMA = 0 self.AIN1 = 1 self.AIN2 = 2 self.PWMB = 5 self.BIN1 = 3 self.BIN2 = 4 def MotorRun(self, motor, index, speed): if speed > 100: return if(motor == 0): pwm.setDutycycle(self.PWMA, speed) if(index == Dir[0]): pwm.setLevel(self.AIN1, 0) pwm.setLevel(self.AIN2, 1) else: pwm.setLevel(self.AIN1, 1) pwm.setLevel(self.AIN2, 0) else: pwm.setDutycycle(self.PWMB, speed) if(index == Dir[0]): pwm.setLevel(self.BIN1, 0) pwm.setLevel(self.BIN2, 1) else: pwm.setLevel(self.BIN1, 1) pwm.setLevel(self.BIN2, 0) def MotorStop(self, motor): if (motor == 0): pwm.setDutycycle(self.PWMA, 0) else: pwm.setDutycycle(self.PWMB, 0) class Rober: def __init__(self): rospy.init_node('rober', anonymous=True) self.motor = MotorDriver() self.speed = 0 self.motor.MotorRun(0, Dir[0], self.speed) self.motor.MotorRun(1, Dir[0], self.speed) self.pub = rospy.Publisher('scan', Range, queue_size=10) self.sub = rospy.Subscriber('cmd_vel', Twist, self.callback, queue_size=10) def callback(self, data): self.speed = data.linear.x * 100 rospy.loginfo('set linear.x = {}'.format(self.speed)) if self.speed > 0: self.motor.MotorRun(0, Dir[0], self.speed) self.motor.MotorRun(1, Dir[0], self.speed) elif self.speed == 0: self.motor.MotorStop(0) self.motor.MotorStop(1) else: self.motor.MotorRun(0, Dir[1], -self.speed) self.motor.MotorRun(1, Dir[1], -self.speed) if __name__ == '__main__': try: rober = Rober() rospy.loginfo('start') rospy.spin() except ROSInterruptException: pass
コピーしたら、以下のコマンドでスクリプトの実行権限を与えます。
chmod u+x rober.py
ロボットの操作を行うコード
次にロボットを操作するためのコードを用意します。
といってもx方向にしか対応してないので、ただロボットが前後するだけのシンプルなやつです。
Classを駆使してrospyらしいかっこつけたコードになりましたｗ
以下のコードをcotrol_rober.pyでscriptsディレクトリに保存します。
#!/usr/bin/python3 import time import rospy from rospy.exceptions import ROSInterruptException from geometry_msgs.msg import Twist class Contorller: def __init__(self): rospy.init_node('controller', anonymous=True) self.is_forward = True self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) rospy.Timer(rospy.Duration(1.0), self.timer_callback) # Twistの初期化 twiet = Twist() twiet.linear.x = 0 twiet.angular.z = 0 self.pub.publish(twiet) def timer_callback(self, event): # Twistの初期化 twiet = Twist() if self.is_forward: twiet.linear.x = 0.5 else: twiet.linear.x = -0.5 self.pub.publish(twiet) rate = rospy.Rate(10) rate.sleep() twiet.linear.x = 0 self.pub.publish(twiet) self.is_forward = not self.is_forward if __name__ == '__main__': try: Contorller() rospy.spin() except ROSInterruptException: pass
これも実行権限を与えておきます。
chmod u+x control_rober.py
動かしてみる
それでは、実際にロボットを動かしてみましょう。
本当はロボット側でlaunchファイルを作るのがいいかもしれませんが、面倒なので順番にコマンドを実行しますｗ。
以下のコマンドでroscoreを立ち上げます。
roscore
新たにターミナルを立ち上げて以下のコマンドを実行してロボットを起動します。
cd ~/ros_catkin_ws source devel/setup.bash rosrun my_rober rober.py
もう一つターミナルを立ち上げて以下のコマンドでロボットを操作するプログラムを起動させます。
cd ~/ros_catkin_ws source devel/setup.bash rosrun my_rober control_rober.py
一通り動かすと、以下のように揺れるようにロボットが前後するようになります！
とりあえずの動作確認なので、机の上で動かせるように移動距離を短くしてますが、もう少し長くしたければ rate = rospy.Rate(10)の時間を長くしてもらえば長くなります。
机の上で揺れるように動くようになった！
— KMiura (@k_miura_io) 2021年12月31日
ここまででブログ書こうかな
年またぎそうw#大晦日ハッカソン pic.twitter.com/uHObG1dUXQ
終わりに
今回はRaspberry Piで動かす自作のROS対応ロボット作りの第一歩としてロボットが前後に動かせるようにしました。
めちゃくちゃ初歩ですが、色々詰まってしまいまさかこれだけで1日時間が潰れるとは思いませんでしたw。
今度こそ旋回を実装して簡単なセンサーで障害物回避できるようにしていきます(それを冬休みにやり切るのが目標です…)。
