2022年1発目の投稿です、あけましておめでとうございます！

今年も変わらずブログを投稿していくので変わらずよろしくお願いします。

前回はRaspberry Piを使ったROS対応ロボットカー作りのベースとして前進・後退をするだけの機能を実装しました。

supernove.hatenadiary.jp

で今回は更にロボットらしい機能に近づけるべく、旋回と超音波センサーをロボットから配信できるようにしてみました。

例の如く今回の成果は以下のGithubで公開しています。

成果のコミットが分かるように、今回からブログのネタごとにブランチを切ってみることにしました。

今回のコミットは backlog/step2でブランチを切っています。前回のコミットは backlog/step1で切っています。

github.com

ロボットを旋回するようにする

ロボットの旋回をするときには twist.angular.zの値を送信できるようにします。

なので、self.angle = data.angular.z * 100という感じでcallback関数で扱えるようにしようと思います。

というわけで旋回に対応したrober.pyのコールバック関数は以下の実装にしてみました。

    def callback(self, data):
        self.speed = data.linear.x * 100
        self.angle = data.angular.z * 100
        rospy.loginfo('set linear.x = {}, angular.z = {}'.format(self.speed, self.angle))
 
        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 self.angle > 0:
            self.motor.MotorRun(1, Dir[0], self.angle)
            self.motor.MotorRun(0, Dir[1], self.angle)
        elif self.angle < 0:
            self.motor.MotorRun(1, Dir[1], -self.angle)
            self.motor.MotorRun(0, Dir[0], -self.angle)

これだと data.linear.xとdata.angular.zが0じゃないときは旋回が優先されるようになるはずなのでもう少し良い実装出来ないかな…と考えてます。

動作確認

それでは動作確認です。

まずは以下のコマンドでroscoreを立ち上げます。

roscore

別ターミナルでrober.pyを実行します。

rosrun my_rober rober.py

いつもなら配信用のコードを作成しますが、今回はサクッとコマンドラインで配信をします。

以下のコマンドで旋回の確認ができます。

rostopic pub cmd_vel geometry_msgs/Twist -- '[0.0,0.0,0.0]' '[0.0,0.0,-0.5]'

実行すると、右回りで旋回するようにします。(ACアダプターにつないで確認しててわかりにくかったので動画はとってません…)

Ctrl + Cでトピックの配信を止めれます。

ただ、まだ旋回していると思うので以下のコマンドで停止します。

rostopic pub cmd_vel geometry_msgs/Twist -- '[0.0,0.0,0.0]' '[0.0,0.0,0.0]'

超音波センサーの値をPublishできるようにする

ロボットにはなにかしらセンサーを入れるもんだろ？

ということで障害物回避でよく見かけて、Raspberry Piでも使える超音波センサーを使ってみたいと思います。

使うセンサーは超音波センサーの定番のHC-SR04です。

akizukidenshi.com

配線は以下の図の通りにします。組み立ては(ry (よしなにロボットに組み込んでもらえると…)

f:id:KMiura:20220101231652p:plain

センサーの値を取得してPublishするコードを用意します。

以下の記事のソースコードを参考にROS用にアレンジします。

algorithm.joho.info

パッケージ内の scriptsディレクトリに sonar_sensor.pyとして以下のコードを保存します。

#!/usr/bin/python3
import time
import rospy
from rospy.exceptions import ROSInterruptException
from sensor_msgs.msg import Range
import RPi.GPIO as GPIO

trig_pin = 14
echo_pin = 15

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(trig_pin, GPIO.OUT)
GPIO.setup(echo_pin, GPIO.IN)


class SonarSensor:
    def __init__(self):
        rospy.init_node('rober', anonymous=True)
        rospy.on_shutdown(self.shutdown)
        self.pub = rospy.Publisher('scan', Range, queue_size=10)
        rospy.Timer(rospy.Duration(1.0), self.timer_callback)

    # HIGH or LOWの時計測
    def pulse_in(self, PIN, start=1, end=0):
        if start==0: end = 1
        t_start = 0
        t_end = 0
        # ECHO_PINがHIGHである時間を計測
        while GPIO.input(PIN) == end:
            t_start = time.time()
            
        while GPIO.input(PIN) == start:
            t_end = time.time()
        return t_end - t_start

    # 距離計測
    def calc_distance(self): 
        # TRIGピンを0.3[s]だけLOW
        GPIO.output(trig_pin, GPIO.LOW)
        time.sleep(0.3)
        # TRIGピンを0.00001[s]だけ出力(超音波発射)        
        GPIO.output(trig_pin, True)
        time.sleep(0.00001)
        GPIO.output(trig_pin, False)
        # HIGHの時間計測
        t = self.pulse_in(echo_pin)
        # 距離[cm] = 音速[cm/s] * 時間[s]/2
        v=34000
        distance = v * t/2
        return distance

    def timer_callback(self, data):
        distance = self.calc_distance()
        rospy.loginfo("distance: %f", distance)
        sensor_data = Range()
        sensor_data.range = distance
        self.pub.publish(sensor_data)

    def shutdown(self):
        rospy.sleep(1)
        rospy.loginfo("Shutdown")


if __name__ == '__main__':
    try:
        SonarSensor()
        rospy.spin()
    except ROSInterruptException:
        # ピン設定解除
        GPIO.cleanup()
        pass

動作確認

これも動作確認をしましょう。

以下のコマンドで sonar_sensor.pyを動かします。

rosrun my_rober sonar_sensor.py

動かすとデバッグ用でセンサー値がログに表示されるようになります。

[INFO] [1641048364.638925]: distance: 330.134392
[INFO] [1641048365.638968]: distance: 330.632925
[INFO] [1641048366.637276]: distance: 310.910463
[INFO] [1641048367.637807]: distance: 310.922623
[INFO] [1641048368.638946]: distance: 330.199242
[INFO] [1641048369.637875]: distance: 311.794043
[INFO] [1641048370.637886]: distance: 310.456514

こちらもトピックをコマンドラインで購読してみます。

以下のコマンドで購読します。

rostopic echo scan

購読すると以下のようにセンサーの値が表示されます。

rangeが超音波センサーの値です。

header:
  seq: 1
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
radiation_type: 0
field_of_view: 0.0
min_range: 0.0
max_range: 0.0
range: 310.464630127
---
header:
  seq: 2
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
radiation_type: 0
field_of_view: 0.0
min_range: 0.0
max_range: 0.0
range: 311.327941895
---

さらに control_rober.pyでセンサーの値を購読するようにつくるとこんな感じです。

このコードに関しては次回の実装で紹介しようと思いますが、気になる方は冒頭で紹介したブランチでチェックしてもらえるといいかと思います。

前回のコードにセンサーの値を購読してログ出力するだけのコールバック関数を追加しただけです。

元旦はゆるーくアウトプット
ROSで超音波センサーの距離取ってみた
ここまでくれば障害物回避はあっという間に実装出来そう#ROS pic.twitter.com/ugTFxnr2pc
— KMiura (@k_miura_io) 2022年1月1日

終わりに

今回はロボットに無いと困る旋回とセンサーの値の配信を実装しました。

今回はセンサーの値を配信するコードを別途実装したのですが、これをrober.pyに組み込むかと言うとそうしないつもりです。

sonar_sensor.pyは特に何もいじらずにrober.pyとセットで動かす想定でいますが、毎回それぞれターミナル立ち上げるのは非効率です。

というわけで次回はrober.pyとsonar_sensor.pyを同時に動かすためのlaunchファイルを作成し、センサーの値に合わせてロボットらしく(？)うろうろできるようにしてみたいと思います。

(launchファイルは次回の仕込みで今回のブランチ上にたたき台をコミットしているので興味あれば是非チェックを！)

KMiura's diary

ROSで動かすロボットカー作り(旋回・超音波センサの値を配信できるようになった編)

ロボットを旋回するようにする

動作確認

超音波センサーの値をPublishできるようにする

動作確認

終わりに