前回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は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度回転程度。 舵角制御したいので、指定時間が来たら停止。 |
具体的には、外部ノードが”/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
■ 動作確認
駆動系ノードの動作確認を一通った様子です。
ほぼ、期待通りに動作することを確認できました。
■ 次回
次回は、動画配信のノードを試行していこうと思います。
また、よろしければ見ていただけると嬉しいです。
