TOC
1. はじめに
2. 必要なもの
3. Tello-Driver のインストール
4. ORB-SLAM3-ROS のインストール
はじめに
ROS1 NoeticにTello-DriverとORB-SLAM3-ROSをインストールし、Telloのカメラ映像から単眼SLAM（ORB-SLAM3）を実行します。実行している様子を以下のデモ動画で確認できます。
Env
- Ubuntu 20.04 LTS
- ROS1 Noetic
- Tello EDU
必要なもの
ネイティブの Ubuntu 環境
本記事ではネイティブの Ubuntu 20.04 を使用します。普段Windows PCを使用している方（とくに初めてROSに触れる初心者）は、下記記事を参考にしてネイティブUbuntu環境を用意することをオススメします。
※ WSLを使用した場合、Wi-FIを介したTelloとの通信が上手くいかない可能性があります。
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をインストールしてください。
DJI製トイドローン Tello EDU
本記事ではDJI製の Tello EDU というトイドローンを使用します。機体重量は87g、大きさは10cm×10cmと小型軽量な機体です。機体前方にカメラセンサが搭載されており、映像をリアルタイムで送信することができます。また、機体下方に搭載された赤外線センサーとIMUによってある程度安定した飛行が可能です。
Tello / TelloEDU のスペック
| カメラ | 960×720ピクセル、30fps |
| センサー | 赤外線センサー、IMU |
| 最高速度 | 14km/h（リミッター解除で28km/h） |
| 最高高度 | 30m |
| 機体重量 | 87g（プロペラ、バッテリーを含む） |
執筆時点（2023年2月）では公式サイトから1.7万円で購入できます。
Tello-Driver のインストール
TelloをROSで制御するために Tello-Driver パッケージを使用します。このパッケージには、Wi-Fiを介してドローンと通信するための tello_driver_node と、ドローンをゲームパッドで操縦するための gamepad_teleop_node が含まれます。
また、Telloから送信されるH.264動画ストリームをデコードするために image_transport パッケージを使用します。このパッケージに含まれる image_decompressed は、入力されたsensor_msgs/Image型のメッセージをデコードし、任意のトピック名で再発行します。
1. 依存パッケージのインストール
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. 本体のダウンロード
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行目付近を以下のように書き換えます。
gamepad_teleop_node.py はXboxのゲームパッドのみに対応しているため、自分のゲームパッドに合わせてparse_my_joy関数を書く必要があります。
+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トピックにデコードします。
<?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. ビルド
cd ~/catkin_ws
catkin build -j8
source devel/setup.bash
初心者の方は以下の記事でcatkin buildの使い方を確認してください。
6. 動作確認
tello_driver_node と gamepad_teleop_node を実行し、ゲームパッドでドローンを操縦してみます。また、rviz を実行してTelloのカメラ映像を受信できているか確認します。
roscore
rosrun rviz rviz
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver tello_node.launch
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver joy_teleop.launch
ORB-SLAM3-ROS のインストール
ORB-SLAM3 はORB特徴点を用いる非直接法のVisual SLAMアルゴリズムであり、オープンソースのSLAM実装です。ORB-SLAM3はスタンドアロンライブラリであり、SLAMを実行して得られる自己位置や環境地図の情報をROSトピックとして発行する機能は備わっていないません。そこで本記事では、ORB-SLAM3本体とそれをROSに組み込むためのインターフェイスが一体となっている ORB-SLAM3-ROS パッケージを使用します。
1. 依存パッケージのインストール
ORB-SLAM3はPnagolin・OpenCV・Eigen3に依存しています。Ubuntu 20.04 では OpenCV 4.2 が公式パッケージとして提供されいるため、PangolinとEigen3のみをインストールします。
作業用のフォルダを作成します。
mkdir ros1_ws
cd ros1_ws
Pangolin というGUI用のライブラリをインストールします。Pangolin 0.8を指定してインストールします。
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を指定してインストールします。
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. 本体のダウンロード
cd ~/catkin_ws/src
git clone https://github.com/thien94/orb_slam3_ros.git
3. CMakeファイルの修正
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: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に追加します。
<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. ビルド
sudo apt -y install doxygen libboost-all-dev
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen
cd ~/catkin_ws
catkin build # 時間がかかります
2023.02.30 追記
以下のようなエラーが発生する場合は catkin_pkg をアップグレードしてください。
CMake Warning (dev) at CMakeLists.txt:3 (project):
Policy CMP0048 is not set: project() command manages VERSION variables.
sudo apt upgrade python3-catkin-pkg
6. 動作確認
tello_driver_node と gamepad_teleop_node を実行し、ゲームパッドでドローンを操縦します。
また、orb_slam3_ros を実行してTelloのカメラ映像からSLAMを実行します。orb_slam3_ros を実行した際に、自動的に rviz も実行されます。
roscore
cd ~/catkin_ws
source devel/setup.bash
roslaunch orb_slam3_ros tello.launch
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver tello_node.launch
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver joy_teleop.launch
