ROSでDJI製トイドローン「Tello」を操作してORB-SLAM3を実行する

# TOC
[__1. はじめに__](#はじめに)
[__2. 必要なもの__](#はじめに)
[__3. Tello-Driver のインストール__](#Tello-Driver-のインストール)
[__4. ORB-SLAM3-ROS のインストール__](#ORB-SLAM3-ROS-のインストール)



# はじめに
ROS1 NoeticにTello-DriverとORB-SLAM3-ROSをインストールし、Telloのカメラ映像から単眼SLAM（ORB-SLAM3）を実行します。実行している様子を以下のデモ動画で確認できます。
<iframe width="560" height="315" src="https://www.youtube.com/embed/J0KAULetV0Y" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

>__Env__
>- Ubuntu 20.04 LTS
>- ROS1 Noetic
>- Tello EDU

# 必要なもの
### ネイティブの Ubuntu 環境
本記事ではネイティブの __Ubuntu 20.04__ を使用します。普段Windows PCを使用している方（とくに初めてROSに触れる初心者）は、下記記事を参考にしてネイティブUbuntu環境を用意することをオススメします。
※ WSLを使用した場合、Wi-FIを介したTelloとの通信が上手くいかない可能性があります。

https://qiita.com/Z0E/items/314e053a74279a71f746


### ROS1 Noetic
__ROS__ はオープンソースのロボティクス向けミドルウェアです。ROSには __ROS1__ と __ROS2__ の2つのバージョンがあり、それぞれ年1回ほどのペースでディストリビューションがリリースされています。本記事では __ROS1 Noetic__ を使用します。


　　__ROSディストリビューションと対応するUbuntuバージョン__
| Ubuntu | ROS1 | ROS2 |
| --- | --- | --- |
| Ubuntu 22.04 | ※未対応 | Humble |
| Ubuntu 20.04 | __Noetic__ | Foxy, Galactic |
| Ubuntu 18.04 | Melodic | Bouncy, Crystal, Dashing, Eloquent |
| Ubuntu 17.04 | Lunar, Melodic | Bouncy ※ソースビルドのみ対応 |
| Ubuntu 16.04 | Kinetic, Lunar | Ardent |

ROS初心者の方は、以下の記事を参考にしてROSをインストールしてください。

http://wiki.ros.org/ja/noetic/Installation/Ubuntu


### DJI製トイドローン Tello EDU
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2059946/07caa4a2-ab8a-e437-1f2e-a355d05e5a50.png)
本記事ではDJI製の __Tello EDU__ というトイドローンを使用します。機体重量は87g、大きさは10cm×10cmと小型軽量な機体です。機体前方にカメラセンサが搭載されており、映像をリアルタイムで送信することができます。また、機体下方に搭載された赤外線センサーとIMUによってある程度安定した飛行が可能です。

　　__Tello / TelloEDU のスペック__
| | |
| --- | --- |
| __カメラ__ | 960×720ピクセル、30fps |
| __センサー__ | 赤外線センサー、IMU |
| __最高速度__ | 14km/h（リミッター解除で28km/h）|
| __最高高度__ | 30m |
| __機体重量__ | 87g（プロペラ、バッテリーを含む）|

執筆時点（2023年2月）では公式サイトから1.7万円で購入できます。

https://www.ryzerobotics.com/jp/tello-edu


# Tello-Driver のインストール
TelloをROSで制御するために __Tello-Driver__ パッケージを使用します。このパッケージには、Wi-Fiを介してドローンと通信するための __tello_driver_node__ と、ドローンをゲームパッドで操縦するための __gamepad_teleop_node__ が含まれます。

https://github.com/jordy-van-appeven/tello_driver

また、Telloから送信されるH.264動画ストリームをデコードするために __image_transport__ パッケージを使用します。このパッケージに含まれる __image_decompressed__ は、入力された`sensor_msgs/Image`型のメッセージをデコードし、任意のトピック名で再発行します。


### 1. 依存パッケージのインストール
```bash:Terminal
sudo apt install ros-noetic-cv-bridge
sudo apt install ros-noetic-image-transport
sudo apt install ros-noetic-camera-info-manager
sudo apt install ros-noetic-codec-image-transport
```


### 2. 本体のダウンロード
```bash:Terminal
cd ~/catkin_ws/src
git clone --recursive https://github.com/appie-17/tello_driver.git
git clone https://github.com/ros-perception/camera_info_manager_py.git
```


### 3. Pythonコードの修正
コード内の `#!/usr/bin/env python2` と書かれた箇所をすべて `#!/usr/bin/env python3` に置き換えます。また、__gamepad_teleop_node.py__ の90行目付近を以下のように書き換えます。

:::note
__gamepad_teleop_node.py__ はXboxのゲームパッドのみに対応しているため、自分のゲームパッドに合わせて`parse_my_joy`関数を書く必要があります。
:::

```diff_python:gamepad_teleop_node.py (90~)
+def parse_my_joy(self, msg):
+   if len(msg.buttons) != 11 or len(msg.axes) != 8:
+       raise ValueError('Invalid number of buttons (%d) or axes (%d)' % (
+           len(msg.buttons), len(msg.axes)))
+   self.L1 = msg.buttons[1]
+   self.R1 = msg.buttons[0]
+   self.LX = msg.axes[3]
+   self.LY = msg.axes[4]
+   self.RX = msg.axes[0]
+   self.RY = msg.axes[1]

def parse(self, msg):
    err = None
    try:
        return self.parse_ps3_usb(msg)
-   except ValueError, e:
+   except ValueError as e:
        err = e
    try:
        return self.parse_ps3_usb2(msg)
-   except ValueError, e:
+   except ValueError as e:
        err = e
+   try:
+       return self.parse_my_joy(msg)
+   except ValueError as e:
+       err = e
    raise err
```


### 4. Launchファイルの修正
__gamepad_teleop_node.py__ の90行目付近を以下のように書き換え、__tello_driver_node__ が発行する`/tello/image_raw/h264`トピックのH264動画ストリームを`/tello/image_raw`トピックにデコードします。

```diff_xml:tello_node.launch
<?xml version="1.0"?>
<launch>
	<arg name="tello_ip" default="192.168.10.1" />
	<arg name="tello_cmd_server_port"	default="8889" />
	<arg name="local_cmd_client_port"	default="8890" />
	<arg name="local_vid_server_port"	default="6038" />
	<arg name="camera_calibration"		default="$(find tello_driver)/cfg/960x720.yaml" />    
	<arg name="namespace"			default="tello" />

	<group ns="$(arg namespace)">

		<node pkg="tello_driver" name="tello_driver_node" type="tello_driver_node" output="screen">
			<param name="local_cmd_client_port" value="$(arg local_cmd_client_port)" />
			<param name="local_vid_server_port" value="$(arg local_vid_server_port)" />
			<param name="tello_ip" value="$(arg tello_ip)" />
			<param name="tello_cmd_server_port" value="$(arg tello_cmd_server_port)" />
			<param name="connect_timeout_sec" value="10.0" />
			<param name="stream_h264_video" value="true" />
			<param name="camera_calibration" value="$(arg camera_calibration)" />			
		</node>

-       <node pkg="image_transport" name="image_compressed" type="republish" args="raw in:=image_raw compressed out:=image_raw" />
+       <node pkg="image_transport" name="image_decompressed" type="republish" args="compressed in:=image_raw raw out:=image_raw">
+           <param name="image_transport" value="h264"/>
+       </node>

	</group>

</launch>
```


### 5. ビルド
```bash:Terminal
cd ~/catkin_ws
catkin build -j8
source devel/setup.bash
```
初心者の方は以下の記事でcatkin buildの使い方を確認してください。

https://qiita.com/harumo11/items/ae604ba2e17ffda529c2


### 6. 動作確認
__tello_driver_node__ と __gamepad_teleop_node__ を実行し、ゲームパッドでドローンを操縦してみます。また、__rviz__ を実行してTelloのカメラ映像を受信できているか確認します。

```bash:Terminal 1
roscore
```

```bash:Terminal 2
rosrun rviz rviz
```

```bash:Terminal 3
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver tello_node.launch
```

```bash:Terminal 4
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver joy_teleop.launch
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2059946/d6debe41-de94-2dcd-a542-78e215535044.png)



# ORB-SLAM3-ROS のインストール
__ORB-SLAM3__ はORB特徴点を用いる非直接法のVisual SLAMアルゴリズムであり、オープンソースのSLAM実装です。ORB-SLAM3はスタンドアロンライブラリであり、SLAMを実行して得られる自己位置や環境地図の情報をROSトピックとして発行する機能は備わっていないません。そこで本記事では、ORB-SLAM3本体とそれをROSに組み込むためのインターフェイスが一体となっている __ORB-SLAM3-ROS__ パッケージを使用します。

https://github.com/thien94/orb_slam3_ros


### 1. 依存パッケージのインストール
ORB-SLAM3はPnagolin・OpenCV・Eigen3に依存しています。Ubuntu 20.04 では OpenCV 4.2 が公式パッケージとして提供されいるため、PangolinとEigen3のみをインストールします。

作業用のフォルダを作成します。
```bash:Terminal
mkdir ros1_ws
cd ros1_ws
```

__Pangolin__ というGUI用のライブラリをインストールします。Pangolin 0.8を指定してインストールします。
```bash:Terminal
cd ~/ros1_ws
git clone -b v0.8 --recursive https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
./scripts/install_prerequisites.sh recommended
cmake -B build
cmake --build build # 時間がかかります
```

__Eigen3__ という線形演算を行うライブラリをインストールします。Eigen 3.4.0を指定してインストールします。
```bash:Terminal
cd ~/ros1_ws
git clone -b 3.4.0 https://gitlab.com/libeigen/eigen.git
cd eigen
mkdir build
cd build
cmake ..
make check # とても時間がかかります（やらなくても大丈夫です）
sudo make install
sudo ldconfig
```


### 2. 本体のダウンロード
```bash:Terminal
cd ~/catkin_ws/src
git clone https://github.com/thien94/orb_slam3_ros.git
```


### 3. CMakeファイルの修正
CMakeLists.txt の先頭に以下のように追記して、コンパイラのバージョンを指定します。

```diff_cpp:CMakeLists.txt
+ add_compile_options(-std=c++14)
cmake_minimum_required(VERSION 2.8)
project(ORB_SLAM3)

IF(NOT CMAKE_BUILD_TYPE)
  SET(CMAKE_BUILD_TYPE Release)
ENDIF()i
```


### 4. calibrationファイルの追加
Tello用のcalibrationファイルを作成し、`catkin_ws/src/orb_slam3_ros/config/Monocular`に追加します。

```yaml:calibration_tello.yaml
%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"

Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera1.fx: 517.306408
Camera1.fy: 516.469215
Camera1.cx: 318.643040
Camera1.cy: 255.313989

Camera1.k1: 0.262383
Camera1.k2: -0.953104
Camera1.p1: -0.005358
Camera1.p2: 0.002628
Camera1.k3: 1.163314

# Camera frames per second 
Camera.fps: 30

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Camera resolution
Camera.width: 640
Camera.height: 480

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500.0
```


### 5. Launchファイルの追加
Tello用のLaunchファイルを作成し、`catkin_ws/src/orb_slam3_ros/launch`に追加します。

```xml:tello.launch
<launch>
    <param name="use_sim_time" value="false" />

    <!-- Main node -->
    <node name="orb_slam3" pkg="orb_slam3_ros" type="ros_mono" output="screen">
        <!-- change the topics according to the dataset -->
        <remap from="/camera/image_raw"    to="/tello/image_raw"/>

        <!-- Parameters for original ORB-SLAM3 -->
        <param name="voc_file"      type="string" value="$(find orb_slam3_ros)/orb_slam3/Vocabulary/ORBvoc.txt.bin"/>
        <param name="settings_file" type="string" value="$(find orb_slam3_ros)/config/Monocular/calibration_tello.yaml"/>

        <!-- Parameters for ROS -->
        <param name="world_frame_id"    type="string"   value="world" />
        <param name="cam_frame_id"      type="string"   value="camera" />
        <param name="enable_pangolin"   type="bool"     value="true" />
    </node>
    
    <!-- Visualization -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find orb_slam3_ros)/config/ntuviral_no_imu.rviz" output="screen" />
    
    <!-- Trajectory path -->
    <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_orb_slam3" output="screen" ns="orb_slam3_ros" >
        <param name="/target_frame_name"        value="/world" />
        <param name="/source_frame_name"        value="/camera" />
        <param name="/trajectory_update_rate"   value="20.0" />
        <param name="/trajectory_publish_rate"  value="20.0" />
    </node>
</launch>
```


### 5. ビルド
```bash:Terminal
sudo apt -y install doxygen libboost-all-dev
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen
cd ~/catkin_ws
catkin build # 時間がかかります
```

::: note warn
2023.02.30 追記
以下のようなエラーが発生する場合は `catkin_pkg` をアップグレードしてください。
```bash:エラー
CMake Warning (dev) at CMakeLists.txt:3 (project):
   Policy CMP0048 is not set: project() command manages VERSION variables.
```
```bash:Terminal
sudo apt upgrade python3-catkin-pkg
```
:::


### 6. 動作確認
__tello_driver_node__ と __gamepad_teleop_node__ を実行し、ゲームパッドでドローンを操縦します。
また、__orb_slam3_ros__ を実行してTelloのカメラ映像からSLAMを実行します。__orb_slam3_ros__ を実行した際に、自動的に __rviz__ も実行されます。

```bash:Terminal 1
roscore
```

```bash:Terminal 2
cd ~/catkin_ws
source devel/setup.bash
roslaunch orb_slam3_ros tello.launch
```

```bash:Terminal 3
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver tello_node.launch
```

```bash:Terminal 4
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver joy_teleop.launch
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2059946/d236daa3-5c21-d06b-d172-59c5a54304e5.png)