はじめに
こちらを参考書としてROS2プログラミングの勉強をしていきます．
このページではLiDARからの信号により，前方に障害物がなくなったらロボットを前進させるプログラムを自作します．
環境
・PC: Panasonic Let's Note CF-FV
・OS: Windows11 Pro (64bit)
・VirtualBox: 6.1.38 (Ubuntu20.04.5, GUI環境)
・ROS: ROS2 Foxy
・ロボット：Turtlebot3 burger (Raspberry Pi 3B+)
ここまでの環境構築については下記参照
・Windows PCにVirtualBox＋Ubuntuを導入
https://qiita.com/pez/items/a3ef1855f7e1e0ed3dfd
・ROS(ROS2 Foxy)をインストール
https://qiita.com/pez/items/1df36628524ff40a3d93
・Turtlebot3 burger のセットアップ
https://qiita.com/pez/items/1d3d15b3911d5dab1702
1. ワークスペース作成・準備
今回は lidar_ws としておきます
$ mkdir -p ~/lidar_ws/src/
今回必要なライブラリ
入っていなければ入れておく
sudo apt install ros-foxy-tf-transformations
sudo pip3 install transforms3d
2. パッケージ作成
今回ノード名は my_lidar_node
パッケージ名は my_lidar
にすることにします
$ cd ~/lidar_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_lidar_node my_lidar
3. プログラミング
3.1 package.xml ファイルの編集
$ cd ~/lidar_ws/src/my_lidar
$ emacs package.xml
自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．
3.2 setup.py ファイルの編集
$ cd ~/lidar_ws/src/my_lidar
$ emacs setup.py
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．
entry_point={
'console_scripts': [
'my_lidar_node = my_lidar.my_lidar_node:main'
],
},
※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．
3.3 ソースコード作成
今回もパッケージ作成時に自動的に作られている my_lidar_node.py を編集することにします．
$ cd ~/lidar_ws/src/my_lidar/my_lidar
$ emacs my_lidar_node.py
下記のように記述してみましょう．
import math
import os
import sys
import rclpy
import rospkg
import time
import tf_transformations
from rclpy.node import Node
from rclpy.executors import ExternalShutdownException
from geometry_msgs.msg import Twist, Pose, Point # Twistメッセージ型をインポート
from nav_msgs.msg import Odometry # Odometryメッセージ型をインポート
from sensor_msgs.msg import LaserScan # LaserScanメッセージ型をインポート
from tf_transformations import euler_from_quaternion
from ament_index_python.packages import get_package_share_directory
from rclpy.qos import qos_profile_sensor_data, QoSProfile
class MyLidar(Node): # 簡単なLiDARクラス
def __init__(self): # コンストラクタ
super().__init__('my_lidar_node')
self.timer = self.create_timer(0.01, self.timer_callback)
self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
self.sub = self.create_subscription(Odometry, 'odom', self.odom_cb, 10)
self.x, self.y, self.yaw = 0.0, 0.0, 0.0
self.x0, self.y0, self.yaw0 = 0.0, 0.0, 0.0
self.vel = Twist() # Twistメッセージ型インスタンスの生成
self.set_vel(0.0, 0.0) # 速度の初期化
# LiDARを使うため追加
qos_policy = QoSProfile(reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
history=rclpy.qos.HistoryPolicy.KEEP_LAST,
depth = 5)
self.sub = self.create_subscription(LaserScan, 'scan', self.lidar_cb, qos_profile=qos_policy)
#self.sub = self.create_subscription(LaserScan, 'scan', self.lidar_cb, 1 )
self.scan = LaserScan() # LaserScanメッセージ型インスタンスの生成
self.scan.ranges = [-99.9] * 360 # 取得したデータと区別するためありえない値で初期化
self.snum = 360
def lidar_cb(self, msg): # LiDARのコールバック関数
#print('kokokiteru')
self.scan = msg
print('range(len(msg.ranges))=' + str(len(msg.ranges)))
self.snum = len(msg.ranges)
#time.sleep(1)
for i in range(len(msg.ranges)):
if msg.ranges[i] < msg.range_min or msg.ranges[i] > msg.range_max:
pass # 計測範囲外のデータは使わないような処理が必要．ここではパス．
else:
self.scan.ranges[i] = msg.ranges[i]
def my_lidar(self):
steps = 0
self.set_vel(0.0, 0.0) # 停止
rclpy.spin_once(self) # コールバック関数をよび出す
while rclpy.ok():
print(f'step={steps}')
#Lidarの角度分解能にあわせて調整
nscan = float(self.snum)
li_right = int(270.0*(nscan/360.0))
li_left = int( 90.0*(nscan/360.0))
li_front = 0
li_back = int(180.0*(nscan/360.0))
dist = 0.5 # 前進してOKの距離
if self.scan.ranges[li_front] > dist: #
self.set_vel(0.2, 0.0) # 前進
else:
self.set_vel(0.0, 0.0) # 停止
rclpy.spin_once(self)
#self.print_lidar_info() # scanトピックの値を表示するときはコメントアウト
print(f'r[FRONT]={self.scan.ranges[li_front]}') # 前
print(f'r[ LEFT]={self.scan.ranges[li_left]}') # 左
print(f'r[ BACK]={self.scan.ranges[li_back]}') # 後
print(f'r[RIGHT]={self.scan.ranges[li_right]}') # 右
time.sleep(0.1) # 0.1 [s]
steps += 1
def get_pose(self, msg): # 姿勢を取得する
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
q_x = msg.pose.pose.orientation.x
q_y = msg.pose.pose.orientation.y
q_z = msg.pose.pose.orientation.z
q_w = msg.pose.pose.orientation.w
(roll, pitch, yaw) = tf_transformations.euler_from_quaternion(
(q_x, q_y, q_z, q_w))
return x, y, yaw
def odom_cb(self, msg): # オドメトリのコールバック関数
self.x, self.y, self.yaw = self.get_pose(msg)
self.get_logger().info(
f'x={self.x: .2f} y={self.y: .2f}[m] yaw={self.yaw: .2f}[rad/s]')
def set_vel(self, linear, angular): # 速度を設定する
self.vel.linear.x = linear # [m/s]
self.vel.angular.z = angular # [rad/s]
def move_distance(self, dist): # 指定した距離distを移動する
error = 0.05 # 許容誤差 [m]
diff = dist - math.sqrt((self.x-self.x0)**2 + (self.y-self.y0)**2)
if math.fabs(diff) > error:
self.set_vel(0.25, 0.0)
return False
else:
self.set_vel(0.0, 0.0)
return True
def rotate_angle(self, angle): # 指定した角度angleを回転する
# このメソッドは間違っています．move_distanceを参考に完成させてください．
self.set_vel(0.0, 0.25)
return False
def timer_callback(self): # タイマーのコールバック関数
self.pub.publish(self.vel) # 速度指令メッセージのパブリッシュ
def main(): # main関数
rclpy.init()
node = MyLidar()
try:
node.my_lidar()
except KeyboardInterrupt:
print('Ctrl+Cが押されました．')
finally:
rclpy.shutdown()
4. ビルド
ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます
$ cd ~/lidar_ws
$ colcon build
成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます
5. 設定ファイルの反映
アンダーレイ設定ファイル（ROS2システムの設定）は .bashrc に記述済みとします
※下記が.bashrcに既にある
source /opt/ros/foxy/setup.bash
オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
$ emacs ~/.bashrc
$ source ~/lidar_ws/install/setup.bash
を追加してから反映させる
$ source ~/.bashrc
6. ノードの実行(実機)
$ ros2 launch turtlebot3_bringup robot.launch.py
※ロボットが走り出すので周囲に注意して下さい
$ ros2 run my_lidar my_lidar_node
ロボット前方 0.5m 以内に障害物がなければロボットは前進し続け，障害物があれば停止するという動作をすれば成功です．
7. ノードの実行（シミュレーション）
同じことをシミュレーション上で行うこともできます．
シミュレーションに関するセットアップは別記事参照
https://qiita.com/pez/items/d795978c5436a87bc1b7
$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
$ ros2 run my_lidar my_lidar_node
TurtleBot3の実機を立ち上げている状態でさらにシミュレーションも立ち上げてTeleopを行うと，実機もシミュレーションのロボットも両方同時に動きます．（デジタルツイン？）
8. 練習
Lidarからの障害物情報をもとに，ロボットに色々な動きをさせてみましょう．
→ 障害物をさけながら色々なところを徘徊させられるでしょうか？
またプログラムの中に書いてあるように関数 rotate_angle は未完成なので，指定された角度でロボットを停止させる関数として完成させて下さい
MEMO
＜プログラムを更新したときは＞
1.プログラムの中身を編集して保存
2.ビルド
3.設定ファイルの反映
4.ノードの実行
→ 動作を確認