[実機] ROS 2: Nav2でGPS, IMU, LiDARを使ってGPS Waypoint Followerを動かす
宇宙系のロボット開発サークルで制御の開発をしています。
アメリカで行われる火星探査機の世界大会UniversityRoverChallengeに出場した際、Navigation2を使ってGPSポイントを巡るような探査機型ロボットの制御開発をしていたのでその備忘録になります。
本記事では、Nav2のtutorialにあるnav2_gps_waypoint_follower_demoをベースに、実機のローバーでこのパッケージを実装していく流れを紹介します。
今回取り扱うパッケージはnav_rover_controlという名前で公開しています。 実機のプログラム全体についてはかなり煩雑なため公開していませんがベースは上のプログラムを使用しています。
ローバーに搭載するOBCにはGMKtec「NucBox G5」を採用しました。理由としてはモバイルバッテリーから給電ができ小型で少電力である点、そしてJetsonのようなライブラリやOSの環境依存問題がほぼないためです。
開発環境
| 項目 | 型番 |
|---|---|
| Ubuntu | 22.04 |
| ROS 2 | Iron |
| CPU | Intel N97 |
| GPU | Intel UHD Graphics |
| Memory | 12GB |
| LiDAR | Livox Mid-360 |
| GPS | GEP M-10 |
| IMU + Geomagnetic sensor | BNO055 |
1. シミュレーション用パッケージの編集
- プログラムの編集
シミュレーション用のパッケージが完成したら実機で動かせるようにパッケージを改良していく必要があります。例えば,
- launch_arguments={
- "use_sim_time": "True",
+ launch_arguments={
+ "use_sim_time": "False",
のようにシミュレーション環境ではないためuse_sim_timeの部分をTrueからFalseに変更する必要があります。またgazeboに関する記述も必要ないので取り除きます。
- gazebo_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(launch_dir, 'ares8_rover.launch.py'))
- )
EKFのパッケージも実際に使用するセンサがpublishするtopic名に変更しておきます。
launch_ros.actions.Node(
package="robot_localization",
executable="navsat_transform_node",
name="navsat_transform",
output="screen",
parameters=[rl_params_file, {"use_sim_time": False}],
remappings=[
("imu/data", "livox/imu"),
("gps/fix", "gps/fix"),
("gps/filtered", "gps/filtered"),
("odometry/gps", "odometry/gps"),
("odometry/filtered", "odometry/global"),
],
そのほかにもシミュレーション用の記述の削除やパラメータ設定の変更を行います。
2. 実機センサからのデータ取得
実機での初期段階の動作試験
- LiDAR等のDriverのインストール
私たちは障害物検知のためlivox mid-360 LiDARを導入しました。また物体検知にはrealsenseD435iを使用したためそれらのドライバのインストールを行いました。これらのドライバ内でセンサーデータのpublish周期やtopic名などをカスタマイズして使用しています。これらのパッケージは同じws内でのbuildを行っています。
- TFの追加
シミュレーションではリンク同士(base_link -> gps_link とか)はURDF内で記述されたものがそのままTFとして取得できていました。ただ実機ではURDFを使わなかったため自分たちでTFをpublishする必要がありました。私たちはセンサ取得用のプログラムに直接TFをpublishするプログラムを付け足すことでこの問題を解決しました。最もこれよりも良い方法があるかもしれません。
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion, TransformStamped
from sensor_msgs.msg import Imu, NavSatFix
import serial
import math
from tf2_ros import TransformBroadcaster
class ImuPublisher(Node):
def __init__(self):
super().__init__('imu_publisher')
self.publisher_ = self.create_publisher(Imu, 'imu/data_raw', 10)
self.gps_publisher_ = self.create_publisher(NavSatFix, 'gps/fix', 10)
self.timer = self.create_timer(0.01, self.timer_callback) # 100Hz
# TFブロードキャスターの初期化
self.tf_broadcaster = TransformBroadcaster(self)
# UARTポートの設定
self.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
# データ保存
self.acc = [0.0, 0.0, 0.0]
self.gyro = [0.0, 0.0, 0.0]
self.mag = [0.0, 0.0, 0.0]
self.heading = 0.0
self.latitude = 0.0
self.longitude = 0.0
def timer_callback(self):
while self.ser.in_waiting:
line = self.ser.readline().decode('utf-8', errors='ignore').strip()
self.parse_data(line)
gps_msg = NavSatFix()
gps_msg.header.stamp = self.get_clock().now().to_msg()
gps_msg.header.frame_id = 'gps_link'
gps_msg.latitude = self.latitude
gps_msg.longitude = self.longitude
gps_msg.altitude = 0.0
gps_msg.status.status = 0
gps_msg.status.service = 1 # GPS fix
self.gps_publisher_.publish(gps_msg)
# static tf base_link → gps_link
t_gps = TransformStamped()
t_gps.header.stamp = self.get_clock().now().to_msg()
t_gps.header.frame_id = 'base_link'
t_gps.child_frame_id = 'gps_link'
t_gps.transform.translation.x = 0.0
t_gps.transform.translation.y = 0.0
t_gps.transform.translation.z = 0.0
heading_rad = math.radians(self.heading)
q = self.yaw_to_quaternion(heading_rad)
t_gps.transform.rotation = q
self.tf_broadcaster.sendTransform(t_gps)
# static tf base_link → livox_frame
t_livox = TransformStamped()
t_livox.header.stamp = self.get_clock().now().to_msg()
t_livox.header.frame_id = 'base_link'
t_livox.child_frame_id = 'livox_frame'
t_livox.transform.translation.x = 0.0
t_livox.transform.translation.y = 0.0
t_livox.transform.translation.z = 0.0
heading_rad = math.radians(self.heading)
q = self.yaw_to_quaternion(heading_rad)
t_livox.transform.rotation = q
self.tf_broadcaster.sendTransform(t_livox)
# static tf map → odom
t_map_odom = TransformStamped()
t_map_odom.header.stamp = self.get_clock().now().to_msg()
t_map_odom.header.frame_id = 'map'
t_map_odom.child_frame_id = 'odom'
t_map_odom.transform.translation.x = 0.0
t_map_odom.transform.translation.y = 0.0
t_map_odom.transform.translation.z = 0.0
t_map_odom.transform.rotation.x = 0.0
t_map_odom.transform.rotation.y = 0.0
t_map_odom.transform.rotation.z = 0.0
t_map_odom.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_map_odom)
#
def yaw_to_quaternion(self, yaw):
q = Quaternion()
half_yaw = yaw / 2.0
q.x = 0.0
q.y = 0.0
q.z = math.sin(half_yaw)
q.w = math.cos(half_yaw)
return q
def parse_data(self, line):
try:
if ',' not in line:
return
id_str, value_str = line.split(',')
id_int = int(id_str)
value = float(value_str)
if id_int == 400:
self.gyro[0] = value
elif id_int == 401:
self.gyro[1] = value
elif id_int == 402:
self.gyro[2] = value
elif id_int == 403:
self.acc[0] = value
elif id_int == 404:
self.acc[1] = value
elif id_int == 405:
self.acc[2] = value
elif id_int == 406:
self.mag[0] = value
elif id_int == 407:
self.mag[1] = value
elif id_int == 408:
self.mag[2] = value
elif id_int == 412:
# 北基準（0度）から東基準（0度）に変換
# self.heading = (90.0 - value) % 360.0
self.heading = value
elif id_int == 415:
self.latitude = value
elif id_int == 416:
self.longitude = value
else:
self.get_logger().warn(f"未知のID: {id_int}")
except Exception as e:
self.get_logger().warn(f"パースエラー: '{line}' -> {e}")
def main(args=None):
rclpy.init(args=args)
node = ImuPublisher()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
このプログラムではTFに付け足して、自分たちで作成した基板からUART経由で送られてくるセンサデータをIDごとに分割しGPSや9軸センサのデータを取得しています。それぞれをsensor_msgs.msgの型にしてからデータをpublishしています。これらをEKF_nodeで読み取れるように各パッケージのtopicやframeを編集しています。
3. 実機ローバーの動作
私たちのローバーはUARTからCANへデータを変換し各アクチュエータへ必要なデータを送信しています。そのためNav2から出力される速度と角度のデータをローバーが受け取れる命令形に変換するパッケージを作成しました。
これはシミュレーション時のrqt-graphです。実機ではこの/cmd_velを受け取りローバーが受け取れるデータ形に変換するノードを作成しました。これらのデータはserialポートを通じてローバーへと通信しています。下のプログラムが実際に実装したプログラムです。細かいパラメータを調整した後にUARTへ流しています。
#!/usr/bin/python3
import math
import rclpy
import serial
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from std_msgs.msg import Int16MultiArray
from geometry_msgs.msg import Twist
class Commander(Node):
def __init__(self):
super().__init__('commander')
timer_period = 0.01 # 100Hz
self.sub = self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, 10)
self.pub = self.create_publisher(Int16MultiArray, 'uart_command', 10)
self.rover_control = [0.0, 0.0] # Angle, Speed
self.opposite_degree_R_id = 0x310
self.opposite_degree_L_id = 0x311
self.opposite_spped_id = 0x312
self.angle_can_id = 0x300
self.speed_can_id = 0x300
self.latest_twist = Twist()
self.timer = self.create_timer(timer_period, self.timer_callback)
def cmd_vel_callback(self, msg: Twist):
self.latest_twist = msg
def calculate_angle_and_velocity(self, twist: Twist):
# 並進速度と角速度を取得
linear_x = twist.linear.x
angular_z = twist.angular.z
velocity = linear_x
direction_angle = -angular_z # 右回りを正とするために符号を反転
return direction_angle, velocity
def timer_callback(self):
# 角度と速度を計算
direction_angle, velocity = self.calculate_angle_and_velocity(self.latest_twist)
rover_cmd = Int16MultiArray()
if abs(direction_angle) < 0.2:
self.rover_control[0] = 180
self.rover_control[1] = velocity * 100 * 3.3 + 120 # (-1 ~ 1) * 10 * 3 + 120
elif direction_angle >= 0.2:
self.rover_control[0] = math.degrees(-direction_angle) * 0.4 + 180
self.rover_control[1] = (velocity * 100 * 1.0 + abs(direction_angle) * 38) + 120
self.angle_can_id = self.opposite_degree_R_id
else:
self.rover_control[0] = math.degrees(-direction_angle) * 0.4 + 180
self.rover_control[1] = (velocity * 100 * 1.0+ abs(direction_angle) * 38) + 120
self.angle_can_id = self.opposite_degree_L_id
self.speed_can_id = self.opposite_spped_id
if self.rover_control[1] > 120 + 40:
self.rover_control[1] = 120 + 40
rover_cmd.data = [self.angle_can_id, int(self.rover_control[0]), self.speed_can_id, int(self.rover_control[1])]
# Publish the rover command
self.pub.publish(rover_cmd)
self.rover_control = [0, 0]
def main(args=None):
rclpy.init(args=args)
commander = Commander()
try:
rclpy.spin(commander)
except KeyboardInterrupt:
pass
commander.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
UARTはセンサ受信用のポートとローバーコントロール用のポートを分けることで通信の信頼性向上とプログラムの単純化を図っています。
4. ArUco,YOLO等他パッケージの統合
私たちが出場した大会ではハンマーやペットボトルのような物体やArUcoマーカーと呼ばれる物体に近づいて止まるというミッションが課されていました。そのため複数のパッケージを切り替えて異なるタスクに対応する必要がありました。私たちは監督用のノードを作成し現在のstatusをIDとしてpublishしそれらを各ノードで受け取ることでローバーの状態管理を行いました。今回はArUco nodeとYOLO nodeはnav2を導入せず作成したためGPS Waypoint Followerとは別のアルゴリズムで経路生成を行います。
これは結果としてうまくいきました。実装の難易度も比較的簡単だったため複数人での開発に向いていました。一方BehaviorTreeなどを活用すればより高度な実装もできていたと思うのでこの辺りを今後改良していく予定です。
5. 最後に
ROSを使っての開発はここ1年で始めたのでかなり粗雑な実装になりました。今後はもう少しROSの強みとNav2のポテンシャルを生かした開発をしていきたいと考えています。
Discussion