OSOYOO PI CARキット ROS2化 #1 – ROS2でPI CARの駆動制御を行う

前回ROS2のインストールが完了しましたので、続いて、OSOYOO PI CARキットをROS2を使って走らせようと思います。今回は、下記の順番で進めようと思います。
各ノードの設計については、できるだけ簡潔に説明しようと思いますが、ROS2のお作法的なところは省略させていただきます。

OSOYOO PI CARを動作させる前準備
OSOYOO PI CARはPCA9685 PWMライブラリを使用して駆動モーターを制御します。
PCA9685を使用するためにI2C、GPIOを有効化します。
パッケージの準備
まずは、駆動系、テストノード設計に必要なパッケージを用意します。
OSOYOO PI CARのリソースがPythonですので、まずは、python用を用意します。
駆動系ノード
車輪を駆動して車を移動制御するノードです。
テストノード
駆動系ノードのテストドライバです。

■ OSOYOO PI CARを動作させる前準備
■ パッケージの準備
■ 駆動系ノード
■ テストノード
■ 動作確認
■ 次回

■ OSOYOO PI CARを動作させる前準備

OSOYOO PI CARはPCA9685 PWMライブラリを使用して駆動モーターを制御します。
PCA9685を使用するために必要なI2C、GPIOを有効化します。

〇 raspi-configの有効化

ラズパイのI2Cバスを有効にするためと、今後のデバイスの有効化/無効化を行う際にraspi-configを入れておくと楽なので、下記手順でUnbuntuに導入しておきます。
なお、この時シェルはbashを使用してください。

% bash
$ sudo echo "deb http://archive.raspberrypi.org/debian/ buster main" >> /etc/apt/sources.list
$ sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 7FA3303E
$ sudo apt-get update
$ sudo apt-get install raspi-config
$ sudo mount /dev/mmcblk0p1 /boot

〇 I2Cの有効化

I2Cを有効化するためにrspi-configを起動します。

$ sudo rspi-config

起動後に下記の手順でメニューを選択してI2Cバスを有効化します。

「Interfacing Options」→「I2C」→「Yes」→「Ok」→「Fiinish」

〇 gpio/pca9685 pythonライブラリ導入

車輪モーターの駆動に必要なライブラリ：GPIO/PCA9685を導入します。

$ sudo pip3 install adafruit-gpio
$ sudo pip3 install adafruit-pca9685

■ パッケージの準備

いきなりパッケージという用語が出てきてわかりにくいと思いますが、ROS2はビルド/実行単位をパッケージ毎に行います。プログラムの実行単位(ノード）は、パッケージ内に複数作成することが可能です。このパッケージはプログラム言語毎に作成することになっています。(python用、C++用等々)
今回は、下記のようなパッケージとすることにしました。

パッケージ名	言語	内容
picar_py_pkg	python	python用 pi car パッケージ
picar_cpp_pkg	C++	C++用 pi car パッケージ
picar_interfaces	–	picar制御に必要なメッセージ、サービス定義
picar_bringup	python	launch設定

パッケージ一覧

一応、パッケージは決めましたが、ルールを決めただけでパッケージが必要になった時点で順次作成していこうと思いますが、作成手順を記載します。
今回は、下記のような構成でROS2ワークスペースを構成します。

.
├── build                  ← ROS2環境生成時に自動生成される
├── install                ← 同上
├── log                    ← 同上
└── src                    ← ソースディレクトリ(緑字は、これから生成するパッケージ)
    ├── picar_py_pkg
    ├── picar_cpp_pkg
    ├── picar_interfaces
    └── picar_brinngup

ここでは、ros2_wsをROS2ワークスペースとして取り扱います。
パッケージ生成後に各言語毎におまじないをしなと使えません。生成時に説明しようと思います。

$ cd ros_ws
$ colcon build
$ mkdir src
$ ros2 pkg create --build-type ament_python picar_py_pkg
$ ros2 pkg create --build-type ament_cmake picar_cpp_pkg
$ ros2 pkg create picar_interfaces
$ ros2 pkg create bringup

■ 駆動系ノード

駆動系ノードは、pythonで作成することにしました。
強い理由はないですが、osoyooのリソースがpythonだったくらいの理由です。
picarの駆動制御IFは、外部ノードより「前進」「後退」「左回転」「右回転」の指示に従い、動作するように設計します。今回は、ROS2で標準的に用意されているメッセージgeometry_msgs/twistを使用します。このメッセージは下記のように定義されていますが、便宜上、picar制御用に読み替えて使用します。

Twist メンバー	linear/angularメンバー	picar 制御
linear	x	+方向：前進(0.0 – 1.0) -方向：後進(-1.0 – 0.0) 0:停止数値は、前進/後退スピードを示す
	y	未使用
	z	未使用
angular	x	未使用
	y	未使用
	z	z軸の回転時間を示す。 -方向：右方向の回転。(0.0 – 1.0) +方向：左方向の回転。(-1.0 – 0.0) 数値は、回転時間(秒)を示す。大体１秒で360度回転程度。舵角制御したいので、指定時間が来たら停止。

geometry/twist(“/picar_vehicle/cmd_vel”)

具体的には、外部ノードが”/picar_vehicle/cmd_vel”という名称で発信するメッセージを駆動系ノードでキャッチして指示に従い動作するように設計します。

picar_vehicle：駆動系ノード(PicarVehicleNode)

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

from geometry_msgs.msg import Twist

class PicarVehicleNode(Node):
    def __init__(self):
        super().__init__("picar_vehicle")
        # this get the paramter to set running mode.
        self.declare_parameter("debug", False)
        self.is_debug_ = self.get_parameter("debug").value
        if self.is_debug_ == True:
            self.get_logger().info("debug mode has been started")
        else:
            self.get_logger().info("native mode has been started")

        # this configuration device
        self.pwm_ = PCA9685(address=0x40)   # PCA9685:address = 0x40
        self.pwm_.set_pwm_freq(60)          # 60hz for motor.
        GPIO.setmode(GPIO.BCM)              # is to specify GPIO No.
        GPIO.setwarnings(False)             # without warning.
        self.left_backward_ = 23            # dir = backward.(GPIO23)
        self.left_forward_ = 24             # dir = foward.(GPIO24)
        self.right_backward_ = 27           # dir = backward.(GPIO27)
        self.right_forward_ = 22            # dir = forward.(GPIO22)
        self.en_left_ = 0                   # PCA9685 left channel.
        self.en_right_ = 1                  # PCA9685 right channle.


        # GPIOをOUTPUTモードに設定
        GPIO.setup(self.left_backward_, GPIO.OUT)
        GPIO.setup(self.left_forward_, GPIO.OUT)
        GPIO.setup(self.right_backward_, GPIO.OUT)
        GPIO.setup(self.right_forward_, GPIO.OUT)

        # this starts cmd_vel subscription.
        self.subscriber_ = self.create_subscription(
            Twist, "picar_vehicle/cmd_vel", self.callback_cmd_vel, 10)

        # this starts vehicle controller.
        self.action_unit_ = 0.01
        self.life_time_ = 2.0
        self.x_val_ = 0.0
        self.theta_val_ = 0.0
        self.heart_beat_ = 0.0
        self.vehicle_timer_ = self.create_timer(
            self.action_unit_, self.vehicle_controller)

    def callback_cmd_vel(self, msg):
        self.get_logger().info("linear(" +
            str(msg.linear.x) + "," +
            str(msg.linear.y) + "," +
            str(msg.linear.z) + ") angular(" +
            str(msg.angular.x) + "," +
            str(msg.angular.y) + "," +
            str(msg.angular.z) + ")")

        self.heart_beat_ = self.life_time_
        self.theta_val_ = msg.angular.z
        self.x_val_ = msg.linear.x

    def vehicle_controller(self):
        if self.heart_beat_ <= 0.0:
            self.action_stop()
            return;
        self.heart_beat_ = self.heart_beat_ - self.action_unit_

        if self.theta_val_ < -self.action_unit_:
            self.theta_val_ = self.theta_val_ + self.action_unit_
            if self.is_debug_ == True:
                self.get_logger().info("move right:" + str(self.theta_val_));
            self.action_left(self.theta_val_)
            return
        elif self.theta_val_ > self.action_unit_:
            self.theta_val_ = self.theta_val_ - self.action_unit_
            if self.is_debug_ == True:
                self.get_logger().info("move left:" + str(self.theta_val_));
            self.action_right(self.theta_val_)
            return

        if self.x_val_ > self.action_unit_:
            if self.is_debug_ == True:
                self.get_logger().info("move forward");
            self.action_forward(self.x_val_)
        elif self.x_val_ < -self.action_unit_:
            if self.is_debug_ == True:
                self.get_logger().info("move backward");
            self.action_backward(self.x_val_)
        else:
            self.action_stop()

    def action_forward(self, speed):
        if speed > 1.0:
            speed = 1.0
        GPIO.output(self.left_backward_, GPIO.LOW)
        GPIO.output(self.left_forward_, GPIO.HIGH)
        GPIO.output(self.right_backward_, GPIO.LOW)
        GPIO.output(self.right_forward_, GPIO.HIGH)
        self.pwm_.set_pwm(self.en_left_, 0, int(speed * 4095))
        self.pwm_.set_pwm(self.en_right_, 0, int(speed * 4095))

    def action_backward(self, speed):
        if speed < -1.0:
            speed = -1.0
        GPIO.output(self.left_backward_, GPIO.HIGH)
        GPIO.output(self.left_forward_, GPIO.LOW)
        GPIO.output(self.right_backward_, GPIO.HIGH)
        GPIO.output(self.right_forward_, GPIO.LOW)
        self.pwm_.set_pwm(self.en_left_, 0, int(-speed * 4095))
        self.pwm_.set_pwm(self.en_right_, 0, int(-speed * 4095))

    def action_stop(self):
        GPIO.output(self.left_backward_, GPIO.LOW)
        GPIO.output(self.left_forward_, GPIO.LOW)
        GPIO.output(self.right_backward_, GPIO.LOW)
        GPIO.output(self.right_forward_, GPIO.LOW)
        self.pwm_.set_pwm(self.en_left_, 0, 0)
        self.pwm_.set_pwm(self.en_right_, 0, 0)

    def action_right(self, speed):
        if speed > 1.0:
            speed = 1.0
        GPIO.output(self.left_backward_, GPIO.LOW)
        GPIO.output(self.left_forward_, GPIO.HIGH)
        GPIO.output(self.right_backward_, GPIO.HIGH)
        GPIO.output(self.right_forward_, GPIO.LOW)
        self.pwm_.set_pwm(self.en_left_, 0, int(speed * 4095))
        self.pwm_.set_pwm(self.en_right_, 0, int(speed * 4095))

    def action_left(self, speed):
        if speed < -1.0:
            speed = -1.0
        GPIO.output(self.left_backward_, GPIO.HIGH)
        GPIO.output(self.left_forward_, GPIO.LOW)
        GPIO.output(self.right_backward_, GPIO.LOW)
        GPIO.output(self.right_forward_, GPIO.HIGH)
        self.pwm_.set_pwm(self.en_left_, 0, int(-speed * 4095))
        self.pwm_.set_pwm(self.en_right_, 0, int(-speed * 4095))

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


if __name__ == "__main__":
    main()

PicarVehicleNode メソッド	機能概要
__init__	PicarVehicleNodeの初期化を行う。 PCA9685(PWMコントローラ）、GPIOのセットアップ後サブスクリプション(callback_cmd_vel)、駆動制御（vehicle_controller)を起動します。
callback_cmd_vel	“/picar_vehicle/cmd_vel”サブスクリプションを行います。受け取ったメッセージを駆動制御値に変換します。
vehicle_controller	駆動制御値に従い、picarを前進、後退、左回転、右回転、停止します。それぞれの制御には以下のメソッドを呼び出して制御します。
action_forward	指定されたspeedで前進します。
action_backward	指定されたspeedで後退します。
action_stop	picarを停止します。
action_right	picarを右回転します。
action_left	picarを左回転します。

■ テストノード

駆動系ノードIFにgeometry/twistを使用しましたので、テストノードは手を抜いてROS2標準搭載のノード(teleop_twist_keyboard)を下記のように実行します。
ros2 runはノードの実行を行います。
下記は、「teleop_twist_keyboardパッケージのteleop_twist_keyboardノードを実行し、twistメッセージは、”/picar_vehicle/cmd_vel”で送信する。」ということなります。
あとは、キー操作でpicarが意図通りに動けば、駆動系ノードの動作確認完了です。

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard cmd_vel:="/picar_vehicle/cmd_vel"
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently:	speed 0.5	turn 1.0