Go to Qiita Advent Calendar 2024 Top
LoginSignup
17
13

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@Z0E

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

Last updated at Posted at 2023-02-11

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

image.png
本記事では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. 依存パッケージのインストール

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. 本体のダウンロード

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行目付近を以下のように書き換えます。

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

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トピックにデコードします。

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. ビルド

Terminal
cd ~/catkin_ws
catkin build -j8
source devel/setup.bash

初心者の方は以下の記事でcatkin buildの使い方を確認してください。

6. 動作確認

tello_driver_node と gamepad_teleop_node を実行し、ゲームパッドでドローンを操縦してみます。また、rviz を実行してTelloのカメラ映像を受信できているか確認します。

Terminal 1
roscore
Terminal 2
rosrun rviz rviz
Terminal 3
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver tello_node.launch
Terminal 4
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver joy_teleop.launch

image.png

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のみをインストールします。

作業用のフォルダを作成します。

Terminal
mkdir ros1_ws
cd ros1_ws

Pangolin というGUI用のライブラリをインストールします。Pangolin 0.8を指定してインストールします。

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を指定してインストールします。

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. 本体のダウンロード

Terminal
cd ~/catkin_ws/src
git clone https://github.com/thien94/orb_slam3_ros.git

3. CMakeファイルの修正

CMakeLists.txt の先頭に以下のように追記して、コンパイラのバージョンを指定します。

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に追加します。

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に追加します。

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. ビルド

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 # 時間がかかります

2023.02.30 追記
以下のようなエラーが発生する場合は catkin_pkg をアップグレードしてください。

エラー
CMake Warning (dev) at CMakeLists.txt:3 (project):
   Policy CMP0048 is not set: project() command manages VERSION variables.
Terminal
sudo apt upgrade python3-catkin-pkg

6. 動作確認

tello_driver_node と gamepad_teleop_node を実行し、ゲームパッドでドローンを操縦します。
また、orb_slam3_ros を実行してTelloのカメラ映像からSLAMを実行します。orb_slam3_ros を実行した際に、自動的に rviz も実行されます。

Terminal 1
roscore
Terminal 2
cd ~/catkin_ws
source devel/setup.bash
roslaunch orb_slam3_ros tello.launch
Terminal 3
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver tello_node.launch
Terminal 4
cd ~/catkin_ws
source devel/setup.bash
roslaunch tello_driver joy_teleop.launch

image.png

17
13
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
17
13

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?