実習：SLAM, Navigation(Turtlebot3パッケージ) - masuko-shingo/jetson-nano GitHub Wiki

SLAM, Navigation(Turtlebot3パッケージ)

この項目では，地図生成やナビゲーションについて，実習を通して学んでいきます．
実習内容は，

以下に手順，説明を示します．

準備

基本的にはROBOTISのe-manualに近いセットアップを行っていきますが、度々異なる点があるため、手順を記述していきます。
[参考]：https://emanual.robotis.com/docs/en/platform/turtlebot3/slam/

JetsonNano(以下Nano)に、有線LANケーブル(WiFiドングル)、モニタ(HDMIもしくはDisplayPort)、マウス、キーボードが接続されているか確認する。

電源投入
→microUSB Bの場合はケーブルを接続

→ripoバッテリーの場合はOpenCRのスイッチをONに
opencr_switch

PCとJetsonNanoを接続する

JetsonNanoのIPアドレスを確認する

例：ログイン画面

モニタにログイン画面が表示されたら、パスワードを入力

# turtlebot

ログイン後GUIが表示されたら、

doc home Screenshot from 2021-12-08 18-18-51

左側のアイコンが並ぶ部分(以下、doc:ドックと呼称する)にある黒いアイコンを選択 [docの図]
※ docが無い場合は、キーボードのホットキー(windowsマーク等のキー)を押す doc seek-bar Screenshot 2021-12-08 18-17-51

このような画面になるので、画面上部のシークバーに

# terminal

と入力 terminal(以下端末)と言う名前の黒いアイコンを選択する。
選択すると端末が開く doc terminal Screenshot from 2021-12-08 18-18-34

端末に

$ ifconfig

と入力する

ifconfig
ifconfigは、WiFiやEthernet等の接続状況や、IPアドレス等の詳細を表示するコマンド。

doc-ifconfig drawio (1)

wlan0を確認する
赤の下線が引かれたアドレスが、jetsonnnanoのIPアドレスとなっている。

PC、JetsonNano間のssh接続

前項で確認したNanoのIPアドレスを使用してPCとNano間でssh接続を行う。 PC側で端末を開き、
端末に

$ ssh jetsonnano@IPアドレス

と入力初めて接続する場合は以下の例のようなメッセージが表示されるが、yesと打ち、許可をする。
例
doc-firstssh drawio

許可をした後は、以下のようにパスワードの入力を求められるので

$ ssh jetsonnano@IPアドレス
password:

パスワードを入力する。

# turtlebot

以下の図のように、パスワードを入力してもなにも表示されないが、正しく入力されている。
[ssh passwd図を挿入]

IPアドレスが変更されていた場合

PC側

PC側で ROS_MASTER_URI,ROS_HOSTNAMEのIPアドレスを変更する

PC$ vim ~/.bashrc

例

# export ROS_MASTER_URI=http://PCのIPアドレス:11311
# export ROS_HOSTNAME=PCのIPアドレス

変更を反映させる

PC$ source ~/.bashrc

Nano側

Nano側でも同じく、 ROS_MASTER等のIPを変更

Nano$ vim ~/.bashrc

# export ROS_MASTER_URI=http://PCのIPアドレス:11311
# export ROS_HOSTNAME=JetsonNanoのIPアドレス

変更を反映させる

$ source ~/.bashrc

Bringup

前項の，JetsonNanoとPCを接続するができた後は，turtlebot3のrosパッケージを用いて，bringupを行います．
bringupはロボットを立ち上げるプロセスです．IMUのキャリブレーション等を行います．

準備

まずはJetsonNano(以下Nano)に，有線LANケーブル(WiFiドングル)，LiDAR，OpenCR，モニタ(HDMIもしくはDisplayPort)が接続されているか確認する。

手順

PC側で，roscoreを立ち上げ、NanoとPCのやりとりができるようにする

$ roscore

sshでNanoにログイン
(下記は例)

$ ssh jetsonnano@IPアドレス

ログイン後、/dev/ttyUSB0が存在するか確認

$ ls /dev/ttyUSB0

もしくは

$ cd ~/catkin_ws/src/hls_lfcd_lds_driver/applications
$ ls /dev/ttyUSB0

存在しない場合のメッセージ例

# ls: cannot access '/dev/ttyUSB0': No such file or directory

存在する場合は権限を与える
※存在しない場合はLidarが接続されていない可能性

$ sudo chmod a+rw /dev/ttyUSB0

/dev/ttyACM0が存在するか確認

$ ls /dev/ttyACM0

存在する場合は権限を与える ※存在しない場合はOpenCRが接続されていない可能性

$ sudo chmod 777 /dev/ttyACM0

/dev/ttyACM0、/dev/ttyUSB0に権限を与えたら、bringupする

例

$ roslaunch turtlebot3_bringup turtlebot3_robot.launch 
... logging to /home/jetsonnano/.ros/log/99e71d62-f459-11eb-9fcf-7c7635a4ab69/roslaunch-jetsonnano-desktop-10309.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.0.57:37433/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.11
 * /turtlebot3_core/baud: 115200
 * /turtlebot3_core/port: /dev/ttyACM0
 * /turtlebot3_core/tf_prefix: 
 * /turtlebot3_lds/frame_id: base_scan
 * /turtlebot3_lds/port: /dev/ttyUSB0

NODES
  /
    turtlebot3_core (rosserial_python/serial_node.py)
    turtlebot3_diagnostics (turtlebot3_bringup/turtlebot3_diagnostics)
    turtlebot3_lds (hls_lfcd_lds_driver/hlds_laser_publisher)

ROS_MASTER_URI=http://192.168.0.42:11311

process[turtlebot3_core-1]: started with pid [10321]
process[turtlebot3_lds-2]: started with pid [10322]
process[turtlebot3_diagnostics-3]: started with pid [10323]
[INFO] [1627995276.036103]: ROS Serial Python Node
[INFO] [1627995276.076289]: Connecting to /dev/ttyACM0 at 115200 baud
[INFO] [1627995278.191598]: Requesting topics...
[INFO] [1627995278.221709]: Note: publish buffer size is 1024 bytes
[INFO] [1627995278.228189]: Setup publisher on sensor_state [turtlebot3_msgs/SensorState]
[INFO] [1627995278.243128]: Setup publisher on firmware_version [turtlebot3_msgs/VersionInfo]
[INFO] [1627995278.678712]: Setup publisher on imu [sensor_msgs/Imu]
[INFO] [1627995278.696463]: Setup publisher on cmd_vel_rc100 [geometry_msgs/Twist]
[INFO] [1627995278.834303]: Setup publisher on odom [nav_msgs/Odometry]
[INFO] [1627995278.845176]: Setup publisher on joint_states [sensor_msgs/JointState]
[INFO] [1627995278.857941]: Setup publisher on battery_state [sensor_msgs/BatteryState]
[INFO] [1627995278.869624]: Setup publisher on magnetic_field [sensor_msgs/MagneticField]
[INFO] [1627995280.419722]: Setup publisher on /tf [tf/tfMessage]
[INFO] [1627995280.448152]: Note: subscribe buffer size is 1024 bytes
[INFO] [1627995280.457733]: Setup subscriber on cmd_vel [geometry_msgs/Twist]
[INFO] [1627995280.479522]: Setup subscriber on sound [turtlebot3_msgs/Sound]
[INFO] [1627995280.496880]: Setup subscriber on motor_power [std_msgs/Bool]
[INFO] [1627995280.518429]: Setup subscriber on reset [std_msgs/Empty]
[INFO] [1627995281.563173]: Setup TF on Odometry [odom]
[INFO] [1627995281.585275]: Setup TF on IMU [imu_link]
[INFO] [1627995281.594502]: Setup TF on MagneticField [mag_link]
[INFO] [1627995281.602713]: Setup TF on JointState [base_link]
[INFO] [1627995281.630359]: --------------------------
[INFO] [1627995281.645076]: Connected to OpenCR board!
[INFO] [1627995281.656418]: This core(v1.2.2) is compatible with TB3 Burger
[INFO] [1627995281.665803]: --------------------------
[INFO] [1627995281.683916]: Start Calibration of Gyro
[INFO] [1627995284.123878]: Calibration End

ctrl+alt+t もしくは、 ctrl+shift+tで別端末を開いて

再度ssh通信でNanoにログイン

$ ssh [email protected]
[email protected]'s password: 
Welcome to Ubuntu 18.04.5 LTS (GNU/Linux 4.9.201-tegra aarch64)

 * Documentation:  https://help.ubuntu.com
 * Management:     https://landscape.canonical.com
 * Support:        https://ubuntu.com/advantage
This system has been minimized by removing packages and content that are
not required on a system that users do not log into.

To restore this content, you can run the 'unminimize' command.

47 updates can be applied immediately.
30 of these updates are standard security updates.
To see these additional updates run: apt list --upgradable

Last login: Tue Aug  3 22:52:58 2021 from 192.168.0.42

Teleoperation(遠隔操作)

Bringup でロボットを立ち上げることができたら，
ここからは，Teleoperation(遠隔操作)で動かしていきます．
同じくturltebot3パッケージを用います．

手順

PC側でteleop_keyする

PC$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 
... logging to /home/jetsonnano/.ros/log/99e71d62-f459-11eb-9fcf-7c7635a4ab69/roslaunch-jetsonnano-desktop-10689.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.0.57:36809/

SUMMARY
========

PARAMETERS
 * /model: burger
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /
    turtlebot3_teleop_keyboard (turtlebot3_teleop/turtlebot3_teleop_key)

ROS_MASTER_URI=http://192.168.0.42:11311

process[turtlebot3_teleop_keyboard-1]: started with pid [10701]

Control Your TurtleBot3!
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)

space key, s : force stop

CTRL-C to quit

currently:	linear vel 0.01	 angular vel 0.0 
currently:	linear vel 0.01	 angular vel 0.1 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.1 
currently:	linear vel 0.0	 angular vel 0.2 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.01	 angular vel 0.0 
currently:	linear vel 0.02	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.0	 angular vel 0.0 
currently:	linear vel 0.01	 angular vel 0.0

キーボードの w,a,s,dキーでロボット動かせれば完了です．

備考

sキーで停止できます．

SLAM

ここまでは，ロボットを立ち上げるBringup，キーボードで動かすTeleoperationを行いました．
ここからは，ロボットを動かして，自分の位置推定を行いながら，地図を生成する，SLAMを行います．

rviz画面で地図生成される様子を確認してみましょう
動的障害物(人など)が地図として残るか確認してみましょう

手順

まず，最初にbringupを行います．
PC側で

PC$ roscore

Nano側で，bringupを立ち上げる

PC$ ssh [email protected]
jetsonnano$ roslaunch turtlebot3_bringup turtlebot3_robot.launch

再度PC側 SLAMノードを起動します．

PC$ export TURTLEBOT3_MODEL=burger
PC$ roslaunch turtlebot3_slam turtlebot3_slam.launch

※export TURTLEBOT3_MODEL=burgerは.bashrcに記述している場合は不要
※bringup時，Nano側のROS_MASTER_URIやROS_HOSTNAMEのIPアドレスが正しく設定されていない．もしくは，source ~/.bashrcの打ち忘れがあるとlidarのトピックが飛んでこず、rvizにmap,robotmodelに問題が発生する．対処法 https://github.com/masuko-shingo/jetson-nano/wiki/%E5%AE%9F%E7%BF%92%EF%BC%9ASLAM%2C-Navigation%28Turtlebot3%E3%83%91%E3%83%83%E3%82%B1%E3%83%BC%E3%82%B8%29/#nano%E5%81%B4

別端末、PC側で

PC$ export TURTLEBOT3_MODEL=burger
PC$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

ロボットを動かし、マッピングする
マッピングした地図を保存別端末で

PC$ rosrun map_server map_saver -f ~/map

-fは地図をどこに保存するかのパスを指定できる
この場合/home/（ユーザー名）に保存される
生成されるファイルはmap.pgmとmap.yaml
保存後は名前を変更してわかりやすくする．
※使用する地図の名前は、変更すると動かない場合があるのでmapのままを推奨
(デモ)SLAMを行う様子

Navigation

ロボットの進行方向に障害物を置いて，挙動を観察してみましょう．

Bringup まで同工程 PC側で

PC$ roscore

nano側で

PC$ ssh [email protected]
jetsonnano$ roslaunch turtlebot3_bringup turtlebot3_robot.launch

再度PC側

PC$ export TURTLEBOT3_MODEL=${TB3_MODEL}
PC$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

map_fileはslamで保存した地図を選択する.
※export TURTLEBOT3_MODEL=${TB3_MODEL}は.bashrcに記述している場合は不要
※デフォルトで生成される地図ファイルは、＄HOMEにmap.yamlが生成されるが、名前、保存されている場所(パス)を変更した場合は対応させる。(mapファイルの名前はデフォルトのままでのみ動作を確認した)
rvizが開き、マップ、ロボットモデル、などが表示されていれば成功
doc-navigation-Screenshot from 2022-01-24 01-05-32

rviz画面上部にある2D Pose Estimateをクリックし、ロボットが現在居る位置を設定する。ドラッグすることによって向きを指定できる。

2D Pose Estimateで現在位置を合わせたら、teleopで散らばったパーティクルを収束させる。
url

masuko@masuko-PC-VKM24BZG1:~$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

緑の小さい矢印(パーティクル)がロボットモデルぐらいまで収束するまでランドマークと合わせ続ける
doc-navigation-2-Screenshot from 2022-01-24 01-09-17

パーティクルがおおよそ収束したら，ゴールを設定する.
rviz画面上部にある2D Nav Goalをクリックし、ロボットが現在居る位置を設定する。ドラッグすることによって向きを指定できる。ドラッグを離すと，ナビゲーションが始まる．

(デモ)Navigationの様子

bagファイルの取得

rosbagというコマンドを用いてbagファイルを取得します．
bagファイルは，大まかに説明するとノード，プロセス間のトピックを記録したもので，
ロボットや，プログラムを動作させている状態で録ると，取得できたbagファイルの再生を行うことにより，
動作させた際のロボットやプログラムの状況を確認,することができます．そのため，デバッグ作業等で用いることが多いです．

navigationを行った際のbagファイルを取得し，再生してみましょう．

参考：

http://wiki.ros.org/rosbag/Commandline

rosbag record

$ rosbag record -a

-aというオプションはすべてのトピックを記録するという意味です．取り漏らしがないように，基本的にはこのコマンドを使用します．

play

$ rosbag play

ループ再生させる

$ rosbag play -l recorded1.bag

このコマンドで取得したbagファイルを再生します．オプションにより，再生速度や，ループ再生等が可能です．

rqt_graph

例 Bringupした際のトピックを図示したrqtグラフ
220117turtlebot3bringup_active-topic_rosgraph
この図はrqtグラフとよばれ，ノード間で飛んでいる(publish，subscribeされている)，トピックを図で表したものです．
rqt_graphで，現在ROSで飛んでいるトピックを図示し，視覚化してくれます．

$ rqt_graph

slamノードを立ち上げた状態でrqtグラフを見てみましょう．
参考：

http://wiki.ros.org/rqt_graph

参考文献

ドキュメント

・はじめに
https://github.com/masuko-shingo/jetson-nano/blob/main/README.md
・JetsonNano環境構築
https://github.com/masuko-shingo/jetson-nano/wiki/Jetsonnano%E7%92%B0%E5%A2%83%E6%A7%8B%E7%AF%89
・実習：物体検出(YOLOv3)
https://github.com/masuko-shingo/jetson-nano/wiki/%E5%AE%9F%E7%BF%92%EF%BC%9A%E7%89%A9%E4%BD%93%E6%A4%9C%E5%87%BA(YOLOv3)