OSOYOO PI CARキット ROS2化 #4 – ROS2 超音波距離センサーを搭載する

今回は、遠隔操作に必要なセンサー制御用ノードを追加していきます。
搭載するノードは、超音波距離センサー HC-SR04と、首振り用サーボモーター SG-90を制御します。
現状は下記のように、暫定的に超音波距離センサー、カメラをSG-90で回る首の上に乗っけてあります。

ブラウザからの指示で首を振り、首を振った方向の障害物までの距離を計測します。
OSOYOO PI CARキットでは、駆動モーター、SG-90は、OSOYOO PWM HATを使用してPWM(駆動信号）を生成しています。
今回は、このまま利用することとし、下記のようなノード構成としました。

rqt_graphで表示するとノード構成は、下記のような形態です。/rosbridge_websocketがブラウザとのIFとなります。
超音波センサーからの入力をブラウザーが常に受け取り、首を振る方向もブラウザから指示するようにします。

ノード名称は少々大げさな名前を付けていますが、とりあえず気にせず

■ サーボモータ SG-90の制御（/picar_gimbal)

python上でOSOYOO PWM HATを使ってSG-90を制御するには、「Adafruit CircuitPython」を導入する必要があります。

$ pip3 install adafruit-circuitpython-servokit

あとは、トピックより受信したメッセージに従い、Adafruit CircuitPythonライブラリを使って、角度指定を行えば動作します。
メッセージは、駆動用モータ制御と同様の標準メッセージ(geometry_msgs/msg/Twist)を使用します。今回は水平方向に90度回るだけの首ですが、立体的に動作する機構の首でも使えるようなメッセージにしておきました。
こうしておくことで、駆動系テストにも使用したROS2標準ユーティリティ：teleop_twist_keyboardでテストすることも可能となる付加価値も生まれます。

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard cmd_vel:="/picar_gimbal/cmd_vel"

作成したノードは下記のモノとなります。
twist.angular.zの指示に従って、0:正面 -1:左45° 1:右45°となるようにSG-90を制御します。

#!/usr/bin/env python3
import rclpy
import time
from rclpy.node import Node
from adafruit_servokit import ServoKit

from geometry_msgs.msg import Twist

class PicarGimbalNode(Node):
    def __init__(self):
        super().__init__("picar_gimbal")

        # this configuration device
        self.servo_ = ServoKit(channels=16) # is to generate pwm ch.
        self.servo_control(0.0)             # is to set center.

        # this starts cmd_vel subscription.
        self.subscriber_ = self.create_subscription(
            Twist, "picar_gimbal/cmd_vel", self.callback_cmd_vel, 10)
        self.get_logger().info("Gimbal subscription has been started")

    def callback_cmd_vel(self, msg):
        self.servo_control(msg.angular.z)

    def servo_control(self, angle):
        if angle > 1.0:
            angle = 1.0
        elif angle < -1.0:
            angle = -1.0
        self.servo_.servo[15].angle = 90 - int(45 * angle)

def main(args=None):
    rclpy.init(args=args)
    node = PicarGimbalNode()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

■ 超音波距離センサー HC-SR04の制御（/picar_rangefinder)

超音波距離センサーは、GPIOを使って音波の反響時間を計測するだけのノードとなります。
先に駆動制御した際に、GPIOを入れていますので、改めて何かをインストールする必要はありません。
方式的には、linux ユーザーランド&pythonでは、精度的に厳しい感じがしないでもないでないですが、OSOYOO PI CARキットでとられていた方法を踏襲します。
ですが、少々タイミングがきわどくストールする可能性がありますので、フェールセーフをつけておきました。失敗したときには近距離に物体検知したことにしておきます。（緑部分）
このノードは、0.3秒おきに計測後、距離データ(mm単位）を発信するようにして置き、必要なノードが必要なタイミングで受信するような設計としました。
なお、HC-SR04は、たまに測定時に外れたような値を算出することがあります。
とびぬけた影響をなるべく減らすために常に移動平均をとりながら値を算出しています。

#!/usr/bin/env python3
import rclpy
import time
from rclpy.node import Node
import RPi.GPIO as GPIO

from std_msgs.msg import Int32

class PicarRangeFinderNode(Node):
    def __init__(self):
        super().__init__("picar_rangefinder")

        # this configuration device
        GPIO.setmode(GPIO.BCM)              # is to specify GPIO No.
        GPIO.setwarnings(False)             # without warning.
        self.usonic_trigger_ = 20           # ultrasonic trigger.
        self.usonic_echo_  = 21             # ultrasonic echo.
        GPIO.setup(self.usonic_trigger_, GPIO.OUT)
        GPIO.setup(self.usonic_echo_, GPIO.IN)

        # this is the array to calculate distance.
        self.list_distance_ = [0, 0, 0] # is the array of distance.
        self.num_list_distance_ = 0

        # this starts distance  publisher.
        self.distance_publisher_ = self.create_publisher(
            Int32, "picar_rangefinder/distance", 10)
        self.timer_ = self.create_timer(0.5, self.publish_distance)
        self.get_logger().info("Range finder publisher has been started")

    def publish_distance(self):
        msg = Int32()
        msg.data = int(self.measure() * 10)
        self.distance_publisher_.publish(msg)

    def measure(self):
        GPIO.output(self.usonic_trigger_, True)
        time.sleep(0.00001)
        GPIO.output(self.usonic_trigger_, False)
        st = time.time()
        while GPIO.input(self.usonic_echo_) == 0:
            if (time.time() - st) > 0.1:
                return 0

        start = time.time()
        while GPIO.input(self.usonic_echo_) == 1:
            if (time.time() - start) > 0.1:
                return 0

        stop = time.time();
        elapsed = stop - start
        distance = (elapsed * 34300) / 2
        if self.num_list_distance_ >= 3:
            self.list_distance_[0] = self.list_distance_[1]
            self.list_distance_[1] = self.list_distance_[2]
            self.list_distance_[2] = distance
        else:
            self.list_distance_[self.num_list_distance_] = distance
            self.num_list_distance_ = self.num_list_distance_ + 1

        sum = 0
        for i in range(3):
            sum = sum + self.list_distance_[i]
        distance = sum / self.num_list_distance_
        return distance


def main(args=None):
    rclpy.init(args=args)
    node = PicarRangeFinderNode()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

■ 各ノードのテスト

〇 サーボモーターSG90のテスト

下記のようにROS2標準のツールを使って、思い通りの角度となるかを確認しました。
厳密な確認はせず、雰囲気的に大体の角度は出ていることだけ確認しました。

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard cmd_vel:="/picar_gimbal/cmd_vel"

〇 超音波距離センサー HC-SR04のテスト

下記に実行結果を示します。距離データ(mm)が送信されることを確認しました。

$ ros2 topic echo /picar_rangefiner/distance
data: 555
---
data: 555
---
data: 555
---
data: 556
---
data: 556
---
data: 556
---
data: 555
:

■ 次回

今回は、一通り遠隔操作するために必要な部品の用意が完了しました。
次回は、起動するものが増えてきましたので、launchを用意します。
また、できれば遠隔操作に必要なGUIまでを搭載し、実際に遠隔操作を行ってみようと思います。
また、よろしければ見ていただけるとありがたいです。