LoginSignup
6
6

More than 1 year has passed since last update.

@shmpwk

ドローンをモーションキャプチャを使って飛ばす

Last updated at Posted at 2019-04-04

目的

この記事は、crazyflieというドローンをROSで飛ばす手順が書かれています。
今回は、モーションキャプチャのCortexでドローンの位置測定をしています。OptiTrackやViconにも応用ができます。
PCセットアップからドローンの位置制御まで、一連の流れが参考になれば幸いです。

概要

大まかな内容をまとめたスライドを参照して下さい。
https://docs.google.com/presentation/d/1E152kEJSq8OroU5CQDwn-Ek_MAXmywVpxCge0MjDRZM/edit?usp=sharing

目次

  • 0章　Setup
    • デュアルブート
    • ROS準備
    • pythonSDKでCrazyflieを飛ばす
  • 1章　1つ目のdemo
    • joystickでcrazyflieを操縦する
  • 2章　2つ目のdemo
    • Cortexの使用法
    • VRPNの環境構築
    • モーキャプでcrazyflieを飛ばす

  • 3章　3つ目のdemo

    • 複数のwaypointに飛ばす

  • 4章　安定制御

  • 5章　Crazyswarm

  • 6章　その他

  • 7章　まとめ

使用機器

  • crazyflie2.0, crazyflie2.1
  • Cortex7
  • PC 2台(Cortex用とROS用)
  • 配線コードとか

0. Setup

デュアルブート

  • ros kineticを動かす際は、最新のubuntu18.04よりも16.04を使った方がよい。18.04はkineticにあまり対応していない。

  • 基本的に下記のURLの4節までやった。
    https://qiita.com/medalotte/items/4bb5cfa709e93d044f1c
    不安な箇所は下記のURLを参照した。こちらはより詳しく書かれているが、下記の引用のように少し前提が異なる。

    ※私の図の場合元々入っているOSがubuntuになっていますが、これは、この記事の説明のために、ゴミにする予定のHDDにubuntuを入れて、さらにパーティション分割してubuntuを入れるという、傍からみたらバカみたいな作業をしています。

    http://shimaphoto03.com/program/dual-boot-ubuntu/

  • CSMはdisabled,Secure Bootはenabledにした。
    https://freesoft.tvbok.com/tips/efi_installation/uefi_28_important.html

  • 無事完了した。

  • ただし、後に理由記すが、Secure Bootはdisableにしなければならないことが判明した。よって、CSMはenabled, Secure Bootはdisabledにすべきだったかもしれない。

rosインストール

http://wiki.ros.org/kinetic/Installation/Ubuntu
上記のURL通りに従った。Tutorialのturtlebotをジョイスティックで動かすところまでも念のため確認して、正常にインストールされていることを確認した。

crazyflie2.0を公式ソフト(python SDK)を使って動かす。

以下の公式ページを参照。
https://www.bitcraze.io/getting-started-with-the-crazyflie-2-0/#inst-comp
LinuxでVirtualBoxをインストールし、BitcrazeVMをインストールする。次に、コントローラーやアンテナを接続する。Crazyflie Clientを起動させることで、そのアプリケーションでドローンを制御する。
Windows10は上のリンク通りに従っていけばすぐに動かせる。しかし、Linuxだとひと手間加えないとうまくいかなかった。Linuxで動かす場合、以下のリンクも参照。
https://books.google.co.jp/books?id=yYQlDwAAQBAJ&pg=PA91&lpg=PA91&dq=rosrun+crazyflie_tools+scan&source=bl&ots=rIQGsZEh-_&sig=ACfU3U3lh1nl-VL1DtleMguCJV3fA4LqEQ&hl=ja&sa=X&ved=2ahUKEwjRzY_0zN3gAhVPEqYKHZbFDwQQ6AEwA3oECAYQAQ#v=onepage&q=rosrun%20crazyflie_tools%20scan&f=false

解決法1.1

セキュアブートを無効にする。
https://ubuntuforums.org/showthread.php?t=2350741
BIOSいじったらそのエラー消えた。

問題点1.2

BitcrazeVMを起動しようとすると以下のエラー。

Failed to open a session for the virtual machine BitcrazeVM.

Implementation of the USB 2.0 controller not found!

Because the USB 2.0 controller state is part of the saved VM state, the VM cannot be started. To fix this problem, either install the 'Oracle VM VirtualBox Extension Pack' or disable USB 2.0 support in the VM settings.

Note! This error could also mean that an incompatible version of the 'Oracle VM VirtualBox Extension Pack' is installed (VERR_NOT_FOUND).

Result Code: NS_ERROR_FAILURE (0x80004005)
Component: ConsoleWrap
Interface: IConsole {872da645-4a9b-1727-bee2-5585105b9eed}

解決法1.2

VirtualBox Extension PackというプラグインをインストールしVirtual Boxに追加。
http://www.oracle.com/technetwork/server-storage/virtualbox/downloads/index.html?ssSourceSiteId=otnjp#extpack

問題点1.3

USB機器(アンテナやコントローラー)が認識されない

解決法1.3

以下参照
https://qiita.com/civic/items/684c4b82428feb0c4ae1

以下抜粋

USB機器の一覧がでないときは、virtualboxを起動するユーザがvboxusersグループに所属していないとダメです！！（なんと！）
$ sudo gpasswd -a YOUR_USERNAME vboxusers
で追加できます。再起動したほうがいいっぽい。

問題点1.4

cordless rumblepadがubuntuに接続できない

解決法1.4

解決法1.5

問題点1.6

crazyfile clientをダブルクリックしても起動しない

解決法1.6

windows10であったら正常に起動。
Linux(Ubuntu16.04)では未解決。
(今回は、このGUIのアプリケーションではなく、ROSで動かすことを目標としているので、問題を放置することにした。)

問題点1.7

pip3 install -e .

をやると、

Directory '.' is not installable. File 'setup.py' not found.

解決法1.7

setup.py のある階層でコマンドたたけばよい。

ここまでやれば、Linuxでもターミナルでcfclientと打つと、アプリケーションが起動して操作できるようになる。
CF(crazyflie)の電源をいれてconnect。GUIを見ると、CFの傾き具合に同期していた。
あとは、firmwareいれていろいろ設定するのだが省略。

未解決だった、LinuxでVM上のcrazyflie clientをダブルクリックしても動かない問題は、それ自体は解決できていないが、代わりにcrazyflie-clients-pythonでコマンドをたたくことで、GUIが起動できるということで妥協。
本来はROSで動かす予定だったので、pythonSDKから動かすアプローチはここで一旦終了。

crazyflie2.0をROSで動かす。

次の章以降のイントロダクション。
大まかな流れは以下のgithubのREADMEを参照。
https://github.com/whoenig/crazyflie_ros
詳しいことは以下を参照。
http://act.usc.edu/publications/Hoenig_Springer_ROS2017.pdf

vrpn download
https://github.com/ros-drivers/vrpn_client_ros

まずはgithubにデフォルトで載っているdemoを動かすことを目標にする。

1. 1つ目のdemo

以下のリンクの４章以降を参照。
http://act.usc.edu/publications/Hoenig_Springer_ROS2017.pdf

For teleoperation using a joystick, use:roslaunch crazyflie_demo teleop_xbox360.launch uri:=radio://0/100/2M
where the uri specifies the uri of your Crazyflie. You can find valid uris using the scan command in the crazyflie_tools package.

4.1節は、 XBox360 Controllerでcrazyflieを動かす。rvizは自己位置を表示するわけではなく、傾きのみを反映する。

4.2でjoystickをデフォルトのxbox360から他のものに変える。今回はlogicool rumblepad2を使う。

  1. Attach your joystick. This will create a new device file, e.g. /dev/input/js0. You can use dmesg to find details about which device file was used in the system log.

5章は複数台のドローン用の話だから、今回はスキップ。

早速コントローラーでドローンを動かしていく。

roslaunch crazyflie_demo teleop_xbox360.launch uri:=radio://0/100/2M

まず上記のradioのuriを合わせる。そのために、crazyflieの電源をONにして以下のscan commandを使うことでuriを特定できる。

rosrun crazyflie_tools scan

問題点2.1

実行するとエラーが出た。

LIBUSB_ERROR_ACCESS

解決法2.1

USB接続の問題なので以下を参照。
http://ask.xmodulo.com/change-usb-device-permission-linux.html

それぞれで、udevのルールを設定した。

sudo vi /etc/udev/rules.d/99-crazyradio.rules

sudo vi /etc/udev/rules.d/99-crazyflie.rules

実は、python SDKの設定時も同様の設定をしている。今回は、それをスキップしたPCで開発しているので、ここで設定せねばならない。
https://books.google.co.jp/books?id=yYQlDwAAQBAJ&pg=PA91&lpg=PA91&dq=rosrun+crazyflie_tools+scan&source=bl&ots=rIQGsZEh-_&sig=ACfU3U3lh1nl-VL1DtleMguCJV3fA4LqEQ&hl=ja&sa=X&ved=2ahUKEwjRzY_0zN3gAhVPEqYKHZbFDwQQ6AEwA3oECAYQAQ#v=onepage&q=rosrun%20crazyflie_tools%20scan&f=false

改めて、scan command。

drone@drone-GL502VSK:~/catkin_ws/src/crazyflie_ros$ rosrun crazyflie_tools scan -v
Found Crazyradio with version 0.53
radio://0/2/2M
No Crazyflie connected via USB cable found.

radioのuriがradio://0/2/2Mと判明した。(しかし、この後radioのuriが0/80/250Kになったのでそれを使うことにした。)

uriを変える

  • crazyflie_demo teleop_xbox360.launch
  • コマンドプロンプト roslaunch crazyflie_demo teleop_xbox360.launch uri:=radio://0/100/2M

問題点2.2と2.3

roscoreした後

roslaunch crazyflie_demo teleop_xbox360.launch uri:=radio://0/2/2M

(uriは最終的には0/80/250K)

rvizと2つのrqt_plotが表示されているがrvizでエラーが出ているし(問題点2.2)、CFの動きに何も反応しない。(問題点2.3)

WARNING: Package "cfclient" does not follow the version conventions. It should not contain leading zeros (unless the number is 0).
... logging to /home/drone/.ros/log/44e9810c-3b28-11e9-a5c0-28c63f262be5/roslaunch-drone-GL502VSK-4074.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

WARNING: Package "cfclient" does not follow the version conventions. It should not contain leading zeros (unless the number is 0).
started roslaunch server http://localhost:46293/

SUMMARY
========

PARAMETERS
 * /crazyflie/crazyflie_add/enable_logging: True
 * /crazyflie/crazyflie_add/enable_logging_battery: True
 * /crazyflie/crazyflie_add/enable_logging_imu: True
 * /crazyflie/crazyflie_add/enable_logging_magnetic_field: True
 * /crazyflie/crazyflie_add/enable_logging_packets: True
 * /crazyflie/crazyflie_add/enable_logging_pressure: True
 * /crazyflie/crazyflie_add/enable_logging_temperature: True
 * /crazyflie/crazyflie_add/pitch_trim: 0
 * /crazyflie/crazyflie_add/roll_trim: 0
 * /crazyflie/crazyflie_add/tf_prefix: crazyflie
 * /crazyflie/crazyflie_add/uri: radio://0/2/2M
 * /crazyflie/crazyflie_add/use_ros_time: True
 * /crazyflie/joy/dev: /dev/input/js0
 * /crazyflie/quadrotor_teleop/x_axis: 5
 * /crazyflie/quadrotor_teleop/x_velocity_max: 30
 * /crazyflie/quadrotor_teleop/y_axis: 4
 * /crazyflie/quadrotor_teleop/y_velocity_max: -30
 * /crazyflie/quadrotor_teleop/yaw_axis: 1
 * /crazyflie/quadrotor_teleop/yaw_velocity_max: -200
 * /crazyflie/quadrotor_teleop/z_axis: 2
 * /crazyflie/quadrotor_teleop/z_velocity_max: 60000
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /crazyflie/
    crazyflie_add (crazyflie_driver/crazyflie_add)
    crazyflie_demo_controller (crazyflie_demo/controller.py)
    joy (joy/joy_node)
    quadrotor_teleop (crazyflie_demo/quadrotor_teleop)
  /
    crazyflie_server (crazyflie_driver/crazyflie_server)
    rqt_plot2 (rqt_plot/rqt_plot)
    rqt_plot3 (rqt_plot/rqt_plot)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

WARNING: Package "cfclient" does not follow the version conventions. It should not contain leading zeros (unless the number is 0).
process[crazyflie_server-1]: started with pid [4091]
process[crazyflie/crazyflie_add-2]: started with pid [4092]
process[crazyflie/joy-3]: started with pid [4103]
process[crazyflie/quadrotor_teleop-4]: started with pid [4113]
process[crazyflie/crazyflie_demo_controller-5]: started with pid [4132]
process[rviz-6]: started with pid [4155]
process[rqt_plot2-7]: started with pid [4168]
[ INFO] [1551339409.079220665]: wait_for_service /add_crazyflie
[ INFO] [1551339409.080276490]: found /add_crazyflie
process[rqt_plot3-8]: started with pid [4183]
[ INFO] [1551339409.085844694]: Adding radio://0/2/2M as crazyflie with trim(0.000000, 0.000000). Logging: 1, Parameters: 1, Use ROS time: 1
[ INFO] [1551339409.274221556]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[crazyflie/crazyflie_add-2] process has finished cleanly
log file: /home/drone/.ros/log/44e9810c-3b28-11e9-a5c0-28c63f262be5/crazyflie-crazyflie_add-2*.log
terminate called after throwing an instance of 'std::runtime_error'
  what():  timeout
[crazyflie_server-1] process has died [pid 4091, exit code -6, cmd /home/drone/catkin_ws/devel/lib/crazyflie_driver/crazyflie_server __name:=crazyflie_server __log:=/home/drone/.ros/log/44e9810c-3b28-11e9-a5c0-28c63f262be5/crazyflie_server-1.log].
log file: /home/drone/.ros/log/44e9810c-3b28-11e9-a5c0-28c63f262be5/crazyflie_server-1*.log
QXcbWindow: Unhandled client message: "_COMPIZ_TOOLKIT_ACTION"

解決法2.2

モーキャプがないとworld座標を決定できないので、rvizでエラーが出るのは当然らしい。つまり、それは問題ではない。
https://github.com/whoenig/crazyflie_ros/issues/51

解決法2.3

以下を参照
http://act.usc.edu/publications/Hoenig_Springer_ROS2017.pdf

原因の一つは、pythonSDKを起動したままであったこと。もう一つは、いつの間にかradioのuriが変わっていたということ。

以上のデバッグにより、1つ目のdemoが正常に動いた。joystick(コントローラー)を使ってCFを動かすことができるようになった。
rqt_graphは以下のようになる。
(https://i.imgur.com/ejysMfR.png)

2. 2つ目のdemo

チュートリアルの6章以降を参考にする。http://act.usc.edu/publications/Hoenig_Springer_ROS2017.pdf

For hovering at (0,0,1) using VICON, use:
roslaunch crazyflie_demo hover_vicon.launch uri:=radio://0/100/2M frame:=/vicon/crazyflie/crazyflie x:=0 y:=0 z:=1
where the uri specifies the uri of your Crazyflie and frame the tf-frame. The launch file runs vicon_bridge automatically.

viconではなく、cortexでVRPN通信する。チュートリアルでVRPN通信をするモーキャプはoptiだが、cortexも同様に扱ってよい。

cortexの使い方　

initial setup
1. callibrate...
2. select initial callibration
3. L字の玉つき棒を置く。
4. capturevolumeを 2000mm四方,高さ3000mm
5. check if the volume is...
6. T字の玉つき棒を振り回す　
7. Nextボタン
8. (９の時間をpreviousで設定）
9. 緑の領域で覆いつくす(6~8の順番は違うかも)　
11.　finished
その他留意点
threshhold ・・・２５０程度
horizontal line ...認識できる玉の大きさを変えられる

剛体登録
1点1点を対象とするとノイズのせいでうまく追えない。そこで複数の再帰性反射の玉をドローンにつけて剛体登録する。
剛体登録は４点以上でできる。実際は再帰性反射の玉にうまく反応しないことが多いので、6点以上で登録したほうが安心。
そこで、ドローンに6つの玉を付けて剛体登録した。

登録法
1. UMarkersで点を選択
2. New Mrkset
3. creaate cortex prop object
4. nameつける
5. 選択したやつに色が付けばよい。
6. 保存：setup save ...Dドラに保存(今は容量一杯だから)
以前登録したものを使う場合は，Add・Remove 剛体を選択する(4つ以上の点じゃないといけない)

詳しくは長い説明書参照。

出力値はSDkのストリームで見るのが従来の方法

ただ、現状のままだと同じ値が何回も表示されているので、プログラムをいじる必要がある。

VRPNでのデータ通信

VRPNでデータの送受信は、cortexのVRPNの説明書参照。

server側
MAC_VRPN_Server.exeをコマンドプロンプトにドラッグアンドドロップして、続けて送り手のアドレス+受け手のアドレスを書く。ここでは、cortexの出力SDKの設定でローカル(127.0.0.1)に出力しているので、送り手は127.0.0.1。受け手はイーサネットで送る側のアドレス(169.254.91.127)とか(10.1.1.100)など設定によって異なる。

client側
vrpn_print_devices.exeをコマンドプロンプトにドラッグアンドドロップして、半角スペース+受け取るデータの剛体の名前+@+イーサネットの送り手のアドレス。

clientはホストPCも、イーサネットで繋いだリモートPCでも同じ結果。
ただし、windowsでしかできない(と思われる。）Ubuntuならrosで受け取れる。

写真のように出力結果とエラーが交互に出力される。処理が追い付いていないのか。

ROSでVRPNデータを受け取る

vrpn_client_rosのsample.launchのserverのvalueをイーサネットで送り手のアドレス：169.254.91.127にしたら成功。

sample.launch↓

<launch>

  <arg name="server" value="169.254.91.127"/>

  <node pkg="vrpn_client_ros" type="vrpn_client_node" name="vrpn_client_node" output="screen">
    <rosparam subst_value="true">
      server: $(arg server)
      port: 3883

      update_frequency: 100.0
      frame_id: world

      # Use the VRPN server's time, or the client's ROS time.
      use_server_time: false
      broadcast_tf: true

      # Must either specify refresh frequency > 0.0, or a list of trackers to create
      refresh_tracker_frequency: 1.0
      trackers:
      #- FirstTracker
       - stick
      #- SecondTracker
    </rosparam>
  </node>

</launch>

tf_viewframe↓

問題点3.1

VRPNの接続がうまくいかない

解決法3.1

コマンドラインのsample.launch の末尾にserver:=イーサネットの送り手のアドレスをかく。

次に、VRPNのrosのclientが本当に値を受けているのか調べる。

rostopic echo /vrpn_client_node/drone2/pose

値を受け取れていた。
受け取りスピードも遅れがない。
cortexで剛体認識されなくなると、値を出さなくなる。
よって正常に受け取れていると判断できる。

モーキャプと連動してdemoを起動

モーキャプ周りの設定は完了。
次に当初の目的である、demoをlaunchする。

問題点3.2

runtime error になる。



robotモデルのerror、つまりxacroの問題が関与していると思われたが、関係無いと判明した。

下のrqt_graphを見ると、本来はcrazyflie_serverのノードがあるはずだが、存在していない。

解決法3.2

まず、プログラムを書き換える。

  • hover_vrpn.launchの中の
    • ipアドレスを変えた
    • radioのuriを変えた
    • ros_vrpn_clientをvrpn_client_rosにかえた
<?xml version="1.0"?>
<launch>
  <arg name="joy_dev" default="/dev/input/js0" />
  <arg name="ip" value="169.254.91.127" />
  <arg name="port" default="3883" />

  <arg name="uri" default="radio://0/80/250K" />
  <arg name="frame" default="crazyflie" />
  <arg name="x" default="0" />
  <arg name="y" default="0" />
  <arg name="z" default="0.5" />


 <!-- run vrpn client -->
    <node pkg="vrpn_client_ros" name="vrpn_client_node" output="screen" type="vrpn_client_node" required="true">
      <param name="ip" value="$(arg ip)" />
      <param name="port" value="$(arg port)" />
      <param name="frame_id" value="/world" />
      <param name="child_frame_id" value="$(arg frame)" />
    </node>

  • sample.launch でデータの受け渡しをする剛体の名前を書き(drone20やdrone2.1.3など)、sever valueをイーサネットのサーバー側のアドレスに書き換えた。

  • crazyflie_driver/launch/crazyflie_add.launchのradioのuriを変えた。

それでも解決しなかったので質問した。
https://github.com/whoenig/crazyflie_ros/issues/124
すると、コマンド文を書き換えなければならないそうだ。

Actually I misread your initial post. Your uri in the initial launch file is radio://0/80/250K/E7E7E7E701. Given that your uri seems to be radio://0/80/250K instead, use roslaunch crazyflie_demo hover_vrpn.launch uri:=radio://0/80/250K frame:=crazyflie1 x:=0 y:=0 z:=0.5 instead of roslaunch crazyflie_demo hover_vrpn.launch uri:=radio://0/80/250K/E7E7E7E701 frame:=crazyflie1 x:=0 y:=0 z:=0.5.

これでruntime_errorは消えた。

問題点3.3

md5sumがあっていない

drone@drone-GL502VSK:~$ roslaunch crazyflie_demo hover_vrpn.launch uri:=radio://0/80/2M frame:=crazyflie1 x:=0 y:=0 z:=0.5 
... logging to /home/drone/.ros/log/64ccc114-496a-11e9-b01b-28c63f262be5/roslaunch-drone-GL502VSK-4454.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order
xacro.py is deprecated; please use xacro instead
started roslaunch server http://localhost:39091/

SUMMARY
========

PARAMETERS
 * /crazyflie/controller/PIDs/X/integratorMax: 0.1
 * /crazyflie/controller/PIDs/X/integratorMin: -0.1
 * /crazyflie/controller/PIDs/X/kd: 20.0
 * /crazyflie/controller/PIDs/X/ki: 2.0
 * /crazyflie/controller/PIDs/X/kp: 40.0
 * /crazyflie/controller/PIDs/X/maxOutput: 10.0
 * /crazyflie/controller/PIDs/X/minOutput: -10.0
 * /crazyflie/controller/PIDs/Y/integratorMax: 0.1
 * /crazyflie/controller/PIDs/Y/integratorMin: -0.1
 * /crazyflie/controller/PIDs/Y/kd: -20.0
 * /crazyflie/controller/PIDs/Y/ki: -2.0
 * /crazyflie/controller/PIDs/Y/kp: -40.0
 * /crazyflie/controller/PIDs/Y/maxOutput: 10.0
 * /crazyflie/controller/PIDs/Y/minOutput: -10.0
 * /crazyflie/controller/PIDs/Yaw/integratorMax: 0.0
 * /crazyflie/controller/PIDs/Yaw/integratorMin: 0.0
 * /crazyflie/controller/PIDs/Yaw/kd: -20.0
 * /crazyflie/controller/PIDs/Yaw/ki: 0.0
 * /crazyflie/controller/PIDs/Yaw/kp: -200.0
 * /crazyflie/controller/PIDs/Yaw/maxOutput: 200.0
 * /crazyflie/controller/PIDs/Yaw/minOutput: -200.0
 * /crazyflie/controller/PIDs/Z/integratorMax: 1000.0
 * /crazyflie/controller/PIDs/Z/integratorMin: -1000.0
 * /crazyflie/controller/PIDs/Z/kd: 6000.0
 * /crazyflie/controller/PIDs/Z/ki: 3500.0
 * /crazyflie/controller/PIDs/Z/kp: 5000.0
 * /crazyflie/controller/PIDs/Z/maxOutput: 60000.0
 * /crazyflie/controller/PIDs/Z/minOutput: 10000.0
 * /crazyflie/controller/crazyflies: [{'id': 1, 'chann...
 * /crazyflie/controller/frame: crazyflie1
 * /crazyflie/controller/worldFrame: world
 * /crazyflie/crazyflie_add/enable_logging: False
 * /crazyflie/crazyflie_add/enable_logging_battery: True
 * /crazyflie/crazyflie_add/enable_logging_imu: True
 * /crazyflie/crazyflie_add/enable_logging_magnetic_field: True
 * /crazyflie/crazyflie_add/enable_logging_packets: True
 * /crazyflie/crazyflie_add/enable_logging_pressure: True
 * /crazyflie/crazyflie_add/enable_logging_temperature: True
 * /crazyflie/crazyflie_add/pitch_trim: 0
 * /crazyflie/crazyflie_add/roll_trim: 0
 * /crazyflie/crazyflie_add/tf_prefix: crazyflie
 * /crazyflie/crazyflie_add/uri: radio://0/80/2M
 * /crazyflie/crazyflie_add/use_ros_time: True
 * /crazyflie/joy/dev: /dev/input/js0
 * /crazyflie/joystick_controller/use_crazyflie_controller: True
 * /crazyflie/pose/name: goal
 * /crazyflie/pose/rate: 30
 * /crazyflie/pose/x: 0
 * /crazyflie/pose/y: 0
 * /crazyflie/pose/z: 0.5
 * /crazyflie/vrpn_client_node/child_frame_id: crazyflie1
 * /crazyflie/vrpn_client_node/frame_id: /world
 * /crazyflie/vrpn_client_node/ip: 169.254.91.127
 * /crazyflie/vrpn_client_node/port: 3883
 * /robot_description: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /crazyflie/
    baselink_broadcaster (tf/static_transform_publisher)
    controller (crazyflie_controller/crazyflie_controller)
    crazyflie_add (crazyflie_driver/crazyflie_add)
    joy (joy/joy_node)
    joystick_controller (crazyflie_demo/controller.py)
    pose (crazyflie_demo/publish_pose.py)
    vrpn_client_node (vrpn_client_ros/vrpn_client_node)
  /
    crazyflie_server (crazyflie_driver/crazyflie_server)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[crazyflie_server-1]: started with pid [4481]
process[crazyflie/crazyflie_add-2]: started with pid [4482]
process[crazyflie/joy-3]: started with pid [4483]
process[crazyflie/joystick_controller-4]: started with pid [4502]
process[crazyflie/controller-5]: started with pid [4520]
process[crazyflie/pose-6]: started with pid [4535]
[ INFO] [1552905879.021238764]: wait_for_service /add_crazyflie
[ INFO] [1552905879.022456548]: found /add_crazyflie
process[crazyflie/baselink_broadcaster-7]: started with pid [4557]
[ INFO] [1552905879.026591662]: Adding radio://0/80/2M as crazyflie with trim(0.000000, 0.000000). Logging: 1, Parameters: 0, Use ROS time: 1
process[crazyflie/vrpn_client_node-8]: started with pid [4568]
process[rviz-9]: started with pid [4595]
[ INFO] [1552905879.050524300]: Connecting to VRPN server at localhost:3883
[ INFO] [1552905879.082839475]: CF Console: SYS: -------------------------
[ INFO] [1552905879.083610263]: CF Console: ---

[ INFO] [1552905879.084980420]: CF Console: SYS: Crazyflie 2.1 is up and r
[ INFO] [1552905879.085894823]: CF Console: unning!

[ INFO] [1552905879.087291985]: CF Console: SYS: Build 0:7b0041f2edb7 (201
[ INFO] [1552905879.088269446]: CF Console: 9.01) CLEAN

[ INFO] [1552905879.089664296]: CF Console: SYS: I am 0x203937434848501700
[ INFO] [1552905879.091085429]: CF Console: 3E003A and I have 1024KB of fl
[ INFO] [1552905879.091915108]: CF Console: ash!

[ INFO] [1552905879.093317834]: CF Console: CFGBLK: v1, verification [OK]

[ INFO] [1552905879.094711109]: CF Console: DECK_DRIVERS: Found 16 drivers
[ INFO] [1552905879.095421705]: CF Console: 

[ INFO] [1552905879.096786186]: CF Console: DECK_INFO: Found 0 deck memory
[ INFO] [1552905879.097531052]: CF Console: .

[ INFO] [1552905879.098895570]: CF Console: DECK_CORE: 0 deck enumerated

[ INFO] [1552905879.100285362]: CF Console: IMU: BMI088 Gyro I2C connectio
[ INFO] [1552905879.101170833]: CF Console: n [OK].

[ INFO] [1552905879.102584771]: CF Console: IMU: BMI088 Accel I2C connecti
[ INFO] [1552905879.103461251]: CF Console: on [OK]

[ INFO] [1552905879.104859788]: CF Console: IMU: BMP388 I2C connection [OK
[ INFO] [1552905879.105489672]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[ INFO] [1552905879.105598904]: CF Console: ]

[ INFO] [1552905879.106949496]: CF Console: ESTIMATOR: Using estimator 1

[ INFO] [1552905879.108352151]: CF Console: CONTROLLER: Using controller 1
[ INFO] [1552905879.109083243]: CF Console: 

[ INFO] [1552905879.110475524]: CF Console: EEPROM: I2C connection [OK].

[ INFO] [1552905879.111815395]: CF Console: SYS: Free heap: 16056 bytes

[ INFO] [1552905879.114762182]: Requesting parameters...
[ INFO] [1552905879.122299478]: Found variables in cache.

(rviz:4595): Gtk-WARNING **: GModule (/usr/lib/x86_64-linux-gnu/gtk-2.0/2.10.0/immodules/im-ibus.so) initialization check failed: GLib version too old (micro mismatch)

(rviz:4595): Gtk-WARNING **: Loading IM context type 'ibus' failed

(rviz:4595): Gtk-WARNING **: GModule (/usr/lib/x86_64-linux-gnu/gtk-2.0/2.10.0/immodules/im-ibus.so) initialization check failed: GLib version too old (micro mismatch)

(rviz:4595): Gtk-WARNING **: Loading IM context type 'ibus' failed

(rviz:4595): Gtk-WARNING **: GModule (/usr/lib/x86_64-linux-gnu/gtk-2.0/2.10.0/immodules/im-ibus.so) initialization check failed: GLib version too old (micro mismatch)

(rviz:4595): Gtk-WARNING **: Loading IM context type 'ibus' failed
[crazyflie/crazyflie_add-2] process has finished cleanly
log file: /home/drone/.ros/log/64ccc114-496a-11e9-b01b-28c63f262be5/crazyflie-crazyflie_add-2*.log
[ INFO] [1552905879.352645146]: Requesting memories...
[ INFO] [1552905879.360216348]: Memories: 5
[ INFO] [1552905879.372340562]: Ready...
[ INFO] [1552905879.372390251]: Elapsed: 0.293019 s
[INFO] [1552905879.586298]: found update_params service
[INFO] [1552905879.586780]: waiting for emergency service
[INFO] [1552905879.588099]: found emergency service
[INFO] [1552905879.588399]: waiting for land service
[INFO] [1552905879.589543]: found land service
[INFO] [1552905879.589876]: waiting for takeoff service
[INFO] [1552905879.590927]: found takeoff service
[ INFO] [1552905880.052012129]: Connection established
[ERROR] [1552905883.302422963]: client wants service /crazyflie/takeoff to have md5sum d41d8cd98f00b204e9800998ecf8427e, but it has b665b6c83a196e4774268cc26329b159. Dropping connection.
[ERROR] [1552905883.311716]: bad callback: <bound method Controller._joyChanged of <__main__.Controller instance at 0x7fcfbc0c2fc8>>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/drone/catkin_ws/src/crazyflie_ros/crazyflie_demo/scripts/controller.py", line 46, in _joyChanged
    self._takeoff()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
    raise ServiceException("unable to connect to service: %s"%e)
ServiceException: unable to connect to service: remote error reported: client wants service /crazyflie/takeoff to have md5sum d41d8cd98f00b204e9800998ecf8427e, but it has b665b6c83a196e4774268cc26329b159. Dropping connection.

解決法3.3

firmwareのバージョンを変えるぐらいしかないという。

そこで今までインストールしたものを振り返る。

  • Bitcraze VM 2018.01

  • firmware:

    • エラーが出たもの： 2018年で最新のもの　 2017.06 2017.04
    • md5sumのエラーがでにくくなったもの: 2018.01

よって、今回は2018.01のfirmwareを使用することにした。

問題点3.4

tfツリーが一つの流れにならずに分岐をしてしまっている。


[ERROR] [1553058690.975974810]: Exception thrown while processing service call: Could not find a connection between 'world' and 'crazyflie1' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1553058690.978363]: bad callback: <bound method Controller._joyChanged of <__main__.Controller instance at 0x7f6a737bdd88>>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/drone/catkin_ws/src/crazyflie_ros/crazyflie_demo/scripts/controller.py", line 46, in _joyChanged
    self._takeoff()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 515, in call
    responses = transport.receive_once()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 727, in receive_once
    p.read_messages(b, msg_queue, sock)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 353, in read_messages
    self._read_ok_byte(b, sock)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 336, in _read_ok_byte
    raise ServiceException("service [%s] responded with an error: %s"%(self.resolved_name, str))
ServiceException: service [/crazyflie/takeoff] responded with an error: Could not find a connection between 'world' and 'crazyflie1' because they are not part of the same tree.Tf has two or more unconnected trees.

terminate called after throwing an instance of 'tf2::ConnectivityException'
  what():  Could not find a connection between 'world' and 'crazyflie1' because they are not part of the same tree.Tf has two or more unconnected trees.
[crazyflie/controller-5] process has died [pid 4559, exit code -6, cmd /home/drone/catkin_ws/devel/lib/crazyflie_controller/crazyflie_controller __name:=controller __log:=/home/drone/.ros/log/2974abd0-4ace-11e9-a1c8-70886b846f8e/crazyflie-controller-5.log].
log file: /home/drone/.ros/log/2974abd0-4ace-11e9-a1c8-70886b846f8e/crazyflie-controller-5*.log

解決法3.4

質問をして自己解決。
https://github.com/whoenig/crazyflie_ros/issues/126

プログラムのフレーム名をそろえること。コマンドラインのフレーム名も合わせること。

問題点3.5

[ERROR] [1553236991.240235639]: Exception thrown while processing service call: "crazyflie1" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [1553236991.242679]: bad callback: <bound method Controller._joyChanged of <__main__.Controller instance at 0x7fa4955f2d88>>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/drone/catkin_ws/src/crazyflie_ros/crazyflie_demo/scripts/controller.py", line 46, in _joyChanged
    self._takeoff()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 515, in call
    responses = transport.receive_once()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 727, in receive_once
    p.read_messages(b, msg_queue, sock)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 353, in read_messages
    self._read_ok_byte(b, sock)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 336, in _read_ok_byte
    raise ServiceException("service [%s] responded with an error: %s"%(self.resolved_name, str))
ServiceException: service [/crazyflie/takeoff] responded with an error: "crazyflie1" passed to lookupTransform argument source_frame does not exist. 

terminate called after throwing an instance of 'tf2::LookupException'
  what():  "crazyflie1" passed to lookupTransform argument source_frame does not exist. 
[crazyflie/controller-5] process has died [pid 703, exit code -6, cmd /home/drone/catkin_ws/devel/lib/crazyflie_controller/crazyflie_controller __name:=controller __log:=/home/drone/.ros/log/b7ee7eae-4c6d-11e9-809f-d8f2ca3d9a0f/crazyflie-controller-5.log].
log file: /home/drone/.ros/log/b7ee7eae-4c6d-11e9-809f-d8f2ca3d9a0f/crazyflie-controller-5*.log

解決法3.5

ひたすらframeをあわせる

２つ目のdemo成功結果

デバッグ完了。
rvizもエラーなく表示されている。

これで、ゲームコントローラーのスイッチで離陸着陸できるようになった。

その時の動画↓
https://youtu.be/KN6rBiB4a1c

3. 3つ目のdemo

waypointを変えながら飛行する。
3つ目のdemoに関しては7章を参照。6章は着地ゴールがstaticであったのに対し、7章はdynamic waypointである。

  1. teleop_vrpn
    roslaunch crazyflie_demo teleop_vicon.launch
    viconではなく、vrpnバージョンも既に用意されていた。crazyflie_demo/scripts/demo.pyを参照して飛行する。

  2. multi_waypoint
    roslaunch crazyflie_demo multi_waypoint_vicon.launch
    vrpnバージョンも存在していた。
    demo1, demo2.pyを参照して飛行する。

1. teleop_vrpn

飛んだが安定しなかった。2つ目の方が大事なので、改良はしない。

drone@drone-Blade:~$ roslaunch crazyflie_demo teleop_vicon.launch 
... logging to /home/drone/.ros/log/0740f0d8-4c71-11e9-809f-d8f2ca3d9a0f/roslaunch-drone-Blade-26641.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order
xacro.py is deprecated; please use xacro instead
started roslaunch server http://drone-Blade:44049/

SUMMARY
========

PARAMETERS
 * /crazyflie/controller/PIDs/X/integratorMax: 0.1
 * /crazyflie/controller/PIDs/X/integratorMin: -0.1
 * /crazyflie/controller/PIDs/X/kd: 20.0
 * /crazyflie/controller/PIDs/X/ki: 2.0
 * /crazyflie/controller/PIDs/X/kp: 40.0
 * /crazyflie/controller/PIDs/X/maxOutput: 10.0
 * /crazyflie/controller/PIDs/X/minOutput: -10.0
 * /crazyflie/controller/PIDs/Y/integratorMax: 0.1
 * /crazyflie/controller/PIDs/Y/integratorMin: -0.1
 * /crazyflie/controller/PIDs/Y/kd: -20.0
 * /crazyflie/controller/PIDs/Y/ki: -2.0
 * /crazyflie/controller/PIDs/Y/kp: -40.0
 * /crazyflie/controller/PIDs/Y/maxOutput: 10.0
 * /crazyflie/controller/PIDs/Y/minOutput: -10.0
 * /crazyflie/controller/PIDs/Yaw/integratorMax: 0.0
 * /crazyflie/controller/PIDs/Yaw/integratorMin: 0.0
 * /crazyflie/controller/PIDs/Yaw/kd: -20.0
 * /crazyflie/controller/PIDs/Yaw/ki: 0.0
 * /crazyflie/controller/PIDs/Yaw/kp: -200.0
 * /crazyflie/controller/PIDs/Yaw/maxOutput: 200.0
 * /crazyflie/controller/PIDs/Yaw/minOutput: -200.0
 * /crazyflie/controller/PIDs/Z/integratorMax: 1000.0
 * /crazyflie/controller/PIDs/Z/integratorMin: -1000.0
 * /crazyflie/controller/PIDs/Z/kd: 6000.0
 * /crazyflie/controller/PIDs/Z/ki: 3500.0
 * /crazyflie/controller/PIDs/Z/kp: 5000.0
 * /crazyflie/controller/PIDs/Z/maxOutput: 60000.0
 * /crazyflie/controller/PIDs/Z/minOutput: 10000.0
 * /crazyflie/controller/frame: crazyflie20
 * /crazyflie/controller/worldFrame: world
 * /crazyflie/crazyflie_add/enable_logging: False
 * /crazyflie/crazyflie_add/enable_logging_battery: True
 * /crazyflie/crazyflie_add/enable_logging_imu: True
 * /crazyflie/crazyflie_add/enable_logging_magnetic_field: True
 * /crazyflie/crazyflie_add/enable_logging_packets: True
 * /crazyflie/crazyflie_add/enable_logging_pressure: True
 * /crazyflie/crazyflie_add/enable_logging_temperature: True
 * /crazyflie/crazyflie_add/pitch_trim: 0
 * /crazyflie/crazyflie_add/roll_trim: 0
 * /crazyflie/crazyflie_add/tf_prefix: crazyflie
 * /crazyflie/crazyflie_add/uri: radio://0/80/250K
 * /crazyflie/crazyflie_add/use_ros_time: True
 * /crazyflie/joy/dev: /dev/input/js0
 * /crazyflie/joystick_controller/use_crazyflie_controller: True
 * /crazyflie/pose/name: goal
 * /crazyflie/pose/rate: 10
 * /crazyflie/pose/x: 0
 * /crazyflie/pose/y: 0
 * /crazyflie/pose/z: 0.5
 * /robot_description: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /vrpn/child_frame_id: crazyflie20
 * /vrpn/frame_id: /world
 * /vrpn/ip: vicon
 * /vrpn/port: 3883

NODES
  /crazyflie/
    baselink_broadcaster (tf/static_transform_publisher)
    controller (crazyflie_controller/crazyflie_controller)
    crazyflie_add (crazyflie_driver/crazyflie_add)
    joy (joy/joy_node)
    joystick_controller (crazyflie_demo/controller.py)
    pose (crazyflie_demo/publish_pose_teleop.py)
  /
    baselink_broadcaster (tf/static_transform_publisher)
    crazyflie_server (crazyflie_driver/crazyflie_server)
    rviz (rviz/rviz)
    vrpn (vrpn_client_ros/vrpn_client_node)

ROS_MASTER_URI=http://localhost:11311

process[crazyflie_server-1]: started with pid [26661]
process[crazyflie/crazyflie_add-2]: started with pid [26662]
process[crazyflie/joy-3]: started with pid [26670]
/opt/ros/kinetic/lib/python2.7/dist-packages/roslib/packages.py:447: UnicodeWarning: Unicode equal comparison failed to convert both arguments to Unicode - interpreting them as being unequal
  if resource_name in files:
process[crazyflie/joystick_controller-4]: started with pid [26686]
process[crazyflie/controller-5]: started with pid [26702]
process[crazyflie/pose-6]: started with pid [26717]
[ INFO] [1553251408.115380764]: wait_for_service /add_crazyflie
[ INFO] [1553251408.116937368]: found /add_crazyflie
process[crazyflie/baselink_broadcaster-7]: started with pid [26734]
[ INFO] [1553251408.120589382]: Adding radio://0/80/250K as crazyflie with trim(0.000000, 0.000000). Logging: 1, Parameters: 0, Use ROS time: 1
/home/drone/catkin_ws/src/crazyflie_ros/crazyflie_demo/scripts/publish_pose_teleop.py:46: SyntaxWarning: name 'lastData' is assigned to before global declaration
  global lastData
process[vrpn-8]: started with pid [26750]
process[baselink_broadcaster-9]: started with pid [26764]
process[rviz-10]: started with pid [26778]
[ INFO] [1553251408.152331452]: Connecting to VRPN server at localhost:3883
[ INFO] [1553251408.194486923]: CF Console: SYS: -------------------------
[ INFO] [1553251408.196065861]: CF Console: ---

[ INFO] [1553251408.198862904]: CF Console: SYS: Crazyflie 2.0 is up and r
[ INFO] [1553251408.203963273]: CF Console: SYS: Build 0:6ee723ad1144 (201
[ INFO] [1553251408.218073626]: CF Console: DECK_DRIVERS: Found 11 drivers
[ INFO] [1553251408.219439333]: CF Console: 

[ INFO] [1553251408.223042119]: CF Console: .

[ INFO] [1553251408.225808619]: CF Console: DECK_CORE: 1 deck enumerated

[ INFO] [1553251408.228220915]: CF Console: Motion chip is: 0x49

[ INFO] [1553251408.230599758]: CF Console: si pihc noitoM: 0xB6

[ INFO] [1553251408.235523380]: CF Console: AK8963 I2C connection [OK].

[ INFO] [1553251408.238310343]: CF Console: LPS25H I2C connection [OK].

[ INFO] [1553251408.241316570]: CF Console: ESTIMATOR: Using estimator 2

[ INFO] [1553251408.258955220]: Requesting parameters...
[ INFO] [1553251408.290102182]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[crazyflie/crazyflie_add-2] process has finished cleanly
log file: /home/drone/.ros/log/0740f0d8-4c71-11e9-809f-d8f2ca3d9a0f/crazyflie-crazyflie_add-2*.log
[ INFO] [1553251409.153801709]: Connection established
[ INFO] [1553251410.474654855]: Found variables in cache.
[ INFO] [1553251410.937528905]: Requesting memories...
[ INFO] [1553251410.952237793]: Memories: 4
[ INFO] [1553251410.974247691]: Ready...
[ INFO] [1553251410.974309941]: Elapsed: 2.785590 s
[INFO] [1553251411.100426]: found update_params service
[INFO] [1553251411.101675]: waiting for emergency service
[INFO] [1553251411.104994]: found emergency service
[INFO] [1553251411.105816]: waiting for land service
[INFO] [1553251411.108641]: found land service
[INFO] [1553251411.109229]: waiting for takeoff service
[INFO] [1553251411.111954]: found takeoff service
[ INFO] [1553251432.188063071]: Takeoff requested!
[ WARN] [1553251436.907953240]: Link Quality low (0.400000)
[ WARN] [1553251438.908540340]: Link Quality low (0.560000)
[ WARN] [1553251440.907602880]: Link Quality low (0.530000)
[FATAL] [1553251442.492866551]: Emergency requested!
^C[rviz-10] killing on exit
[baselink_broadcaster-9] killing on exit
[vrpn-8] killing on exit
[crazyflie/baselink_broadcaster-7] killing on exit
[crazyflie/pose-6] killing on exit
[crazyflie/controller-5] killing on exit
[crazyflie/joystick_controller-4] killing on exit
[crazyflie/joy-3] killing on exit
[crazyflie_server-1] killing on exit
vrpn_Endpoint::send_pending_reports(): No TCP connection
vrpn_Connection_IP::send_pending_reports:  Closing failed endpoint.
vrpn_Connection::~vrpn_Connection: Connection was deleted while 1 references still remain.
shutting down processing monitor...
... shutting down processing monitor complete
done

２．multi_waypoint_vrpn

今までと同様、プログラムでアドレスなどを直してから実行。

drone@drone-Blade:~$ roslaunch crazyflie_demo multi_waypoint_vicon.launch
... logging to /home/drone/.ros/log/0740f0d8-4c71-11e9-809f-d8f2ca3d9a0f/roslaunch-drone-Blade-5101.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://drone-Blade:32867/

SUMMARY
========

PARAMETERS
 * /crazyflie/controller/PIDs/X/integratorMax: 0.1
 * /crazyflie/controller/PIDs/X/integratorMin: -0.1
 * /crazyflie/controller/PIDs/X/kd: 20.0
 * /crazyflie/controller/PIDs/X/ki: 2.0
 * /crazyflie/controller/PIDs/X/kp: 40.0
 * /crazyflie/controller/PIDs/X/maxOutput: 10.0
 * /crazyflie/controller/PIDs/X/minOutput: -10.0
 * /crazyflie/controller/PIDs/Y/integratorMax: 0.1
 * /crazyflie/controller/PIDs/Y/integratorMin: -0.1
 * /crazyflie/controller/PIDs/Y/kd: -20.0
 * /crazyflie/controller/PIDs/Y/ki: -2.0
 * /crazyflie/controller/PIDs/Y/kp: -40.0
 * /crazyflie/controller/PIDs/Y/maxOutput: 10.0
 * /crazyflie/controller/PIDs/Y/minOutput: -10.0
 * /crazyflie/controller/PIDs/Yaw/integratorMax: 0.0
 * /crazyflie/controller/PIDs/Yaw/integratorMin: 0.0
 * /crazyflie/controller/PIDs/Yaw/kd: -20.0
 * /crazyflie/controller/PIDs/Yaw/ki: 0.0
 * /crazyflie/controller/PIDs/Yaw/kp: -200.0
 * /crazyflie/controller/PIDs/Yaw/maxOutput: 200.0
 * /crazyflie/controller/PIDs/Yaw/minOutput: -200.0
 * /crazyflie/controller/PIDs/Z/integratorMax: 1000.0
 * /crazyflie/controller/PIDs/Z/integratorMin: -1000.0
 * /crazyflie/controller/PIDs/Z/kd: 6000.0
 * /crazyflie/controller/PIDs/Z/ki: 3500.0
 * /crazyflie/controller/PIDs/Z/kp: 5000.0
 * /crazyflie/controller/PIDs/Z/maxOutput: 60000.0
 * /crazyflie/controller/PIDs/Z/minOutput: 10000.0
 * /crazyflie/controller/frame: crazyflie20
 * /crazyflie/controller/worldFrame: world
 * /crazyflie/crazyflie_add/enable_logging: False
 * /crazyflie/crazyflie_add/enable_logging_battery: True
 * /crazyflie/crazyflie_add/enable_logging_imu: True
 * /crazyflie/crazyflie_add/enable_logging_magnetic_field: True
 * /crazyflie/crazyflie_add/enable_logging_packets: True
 * /crazyflie/crazyflie_add/enable_logging_pressure: True
 * /crazyflie/crazyflie_add/enable_logging_temperature: True
 * /crazyflie/crazyflie_add/pitch_trim: 0
 * /crazyflie/crazyflie_add/roll_trim: 0
 * /crazyflie/crazyflie_add/tf_prefix: crazyflie
 * /crazyflie/crazyflie_add/uri: radio://0/80/250K
 * /crazyflie/crazyflie_add/use_ros_time: True
 * /crazyflie/joystick_controller/joy_topic: /joy
 * /crazyflie/joystick_controller/use_crazyflie_controller: True
 * /crazyflie/pose/frame: crazyflie20
 * /crazyflie/vrpn/child_frame_id: crazyflie20
 * /crazyflie/vrpn/frame_id: /world
 * /crazyflie/vrpn/ip: vicon
 * /crazyflie/vrpn/port: 3883
 * /joy/dev: /dev/input/js0
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /crazyflie/
    baselink_broadcaster (tf/static_transform_publisher)
    controller (crazyflie_controller/crazyflie_controller)
    crazyflie_add (crazyflie_driver/crazyflie_add)
    joystick_controller (crazyflie_demo/controller.py)
    pose (crazyflie_demo/demo1.py)
    vrpn (vrpn_client_ros/vrpn_client_node)
  /
    crazyflie_server (crazyflie_driver/crazyflie_server)
    joy (joy/joy_node)

ROS_MASTER_URI=http://localhost:11311

process[crazyflie_server-1]: started with pid [5119]
process[joy-2]: started with pid [5121]
process[crazyflie/crazyflie_add-3]: started with pid [5128]
/opt/ros/kinetic/lib/python2.7/dist-packages/roslib/packages.py:447: UnicodeWarning: Unicode equal comparison failed to convert both arguments to Unicode - interpreting them as being unequal
  if resource_name in files:
process[crazyflie/joystick_controller-4]: started with pid [5144]
process[crazyflie/controller-5]: started with pid [5164]
process[crazyflie/pose-6]: started with pid [5176]
process[crazyflie/vrpn-7]: started with pid [5193]
[ INFO] [1553256569.203040063]: wait_for_service /add_crazyflie
[ INFO] [1553256569.203974904]: found /add_crazyflie
[ INFO] [1553256569.207297047]: Adding radio://0/80/250K as crazyflie with trim(0.000000, 0.000000). Logging: 1, Parameters: 0, Use ROS time: 1
process[crazyflie/baselink_broadcaster-8]: started with pid [5212]
[ INFO] [1553256569.215095226]: Connecting to VRPN server at localhost:3883
[ INFO] [1553256569.263870054]: Requesting parameters...
[ INFO] [1553256569.370131654]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[crazyflie/crazyflie_add-3] process has finished cleanly
log file: /home/drone/.ros/log/0740f0d8-4c71-11e9-809f-d8f2ca3d9a0f/crazyflie-crazyflie_add-3*.
[ INFO] [1553256570.216474858]: Connection established
[ INFO] [1553256571.478375868]: Found variables in cache.
[ INFO] [1553256571.833202923]: Requesting memories...
[ INFO] [1553256571.848478949]: Memories: 4
[INFO] [1553256571.848765]: found update_params service
[INFO] [1553256571.849087]: waiting for emergency service
[INFO] [1553256571.850042]: found emergency service
[INFO] [1553256571.850264]: waiting for land service
[INFO] [1553256571.851161]: found land service
[INFO] [1553256571.851382]: waiting for takeoff service
[INFO] [1553256571.852152]: found takeoff service
[ INFO] [1553256571.869970901]: Ready...
[ INFO] [1553256571.870018945]: Elapsed: 2.621655 s
[ INFO] [1553256579.793474094]: Takeoff requested!
[ WARN] [1553256588.608795146]: Link Quality low (0.700000)
[FATAL] [1553256588.846707726]: Emergency requested!
^C[crazyflie/baselink_broadcaster-8] killing on exit
[crazyflie/vrpn-7] killing on exit
[crazyflie/pose-6] killing on exit
[crazyflie/controller-5] killing on exit
[crazyflie/joystick_controller-4] killing on exit
[crazyflie_server-1] killing on exit
[joy-2] killing on exit
vrpn_Endpoint::send_pending_reports(): No TCP connection
vrpn_Connection_IP::send_pending_reports:  Closing failed endpoint.
vrpn_Connection::~vrpn_Connection: Connection was deleted while 1 references still remain.
shutting down processing monitor...
... shutting down processing monitor complete
done
drone@drone-Blade:~$

最初の飛行は、以下の軌跡に沿って飛行させようとした。しかし、x座標で0.5から-0.5に移動する際に行き過ぎてしまい、そのまま制御不能になって壁に激突した。

#!/usr/bin/env python
from demo import Demo

if __name__ == '__main__':
    demo = Demo(
        [
            #x   ,   y,   z, yaw, sleep
            [0.0 , 0.0, 0.5, 0, 5],
            [0.5 , 0.0, 0.5, 0, 5],
            [-0.5 , 0.0, 1.0, 0, 5],
            [-0.5 , 0.5, 0.5, 0, 5],
            [0.0 , 0.0, 0.5, 0, 0],
            #[0.0 , 1.0, 0.3, 0],
            #[1.0 , 1.0, 0.5, 0],
            #[0.0 , 0.5, 0.2, 0],
            #[0.0 , 0.5, 0.0, 0],
        ]
    )
    demo.run()

2つ目のパターンの飛行として、ジャグリングの軌道を描かせた。

#!/usr/bin/env python
from demo import Demo

if __name__ == '__main__':
    demo = Demo(
        [
            #x   ,   y,   z, yaw, sleep
            [0.0 , 0.0, 0.5, 0, 5],
            [0.5 , 0.0, 0.5, 0, 5],
            [-0.5 , 0.0, 1.0, 0, 5],
            [-0.5 , 0.5, 0.5, 0, 5],
            [0.0 , 0.0, 0.5, 0, 0],
            #[0.0 , 1.0, 0.3, 0],
            #[1.0 , 1.0, 0.5, 0],
            #[0.0 , 0.5, 0.2, 0],
            #[0.0 , 0.5, 0.0, 0],
        ]
    )
    demo.run()

その時の動画

https://youtu.be/fDIlVVmIr3A
コントローラーがよくない。与えたゴール点を一度通り過ぎてから戻ってくる。コントローラーのゲインなどの調整が必要である。

これでチュートリアルの内容は一通り終了した。

4.安定制御

チュートリアル9章にcrazyflieの拡張について書かれている。
Possible Extensionsで

Higher-level behaviors, such as following a trajectory rather than just goal points, could make more interesting flight patterns possible.

また、p24にこんなことが書いてあった。複数個のcrazyflieを動かすときには注意が必要そうだ。

The path for the two Crazyflies should not be overlapping because simple way-point following does not have any time guarantees. Hence, it is possible that thefirst Crazyflie finishes much earlier than the second one, even if the total pathlength and sleep time are the same. This limitation can be overcome by gen-erating a trajectory for each Crazyflie and setting the goal points dynamicallyaccordingly.

現時点では安定的に飛行しないので、より良いコントローラーを使いたい。
このcrazyflie_rosの作成者がissueで時々そのことについてふれてくれているので参考にした。https://github.com/whoenig/crazyflie_ros/issues

具体的には、以下のようなことが書かれていた。

you will need to launch external_position_vicon [Which needs to be slightly changed to support Optitrack]. Then, in another terminal, execute python execute_trajectoy.py figure8withTakeoffAndLanding.csv.

a) Adjust the launch file to the vrpn_client
b) Adjust topics names in publish_external_position (could simply be set to /tf)
c) change frame names (according to motive names) in the high level script.

Waypoint navigation and nice takeoff/landing is already supported; Trajectory following is on its way. To use it, use the following steps:
1. Compile the latest official stm32 firmware using ESTIMATOR=kalman and CONTROLLER=mellinger
2. Use the latest (master-branch) of crazyflie_ros
3. I use the following launch file: https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/launch/external_position_vicon.launch. You would need to change it for VRPN (along with a VRPN version of https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/publish_external_position_vicon.py)
4. Waypoint following can be done using a goTo method in the high-level script (see https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/test_high_level.py#L15-L21 for an example). You can specify a desired duration when the CF should reach its goal - planning, state estimation, and non-linear control are all running on-board.

  • 結論としては、この辺りの安定制御は開発途中で、既存のプログラムをいじっても泥沼にハマる可能性がある。実際、個々のプログラムの関係性が
    よくわからず、自分自身泥沼にハマった。

  • 複数台動かすならcrazyswarmのプログラムを使うべきだとも言っていた。そちらの方が開発が進んでいるのだという。https://crazyswarm.readthedocs.io/en/latest/

5. Crazyswarm

↑InstallationからUsageまで取り組んだ。

以下は最初のデモの結果

drone@drone-Blade:~/crazyswarm$ roslaunch crazyswarm hover_swarm.launch
... logging to /home/drone/.ros/log/fd8370a4-4fb1-11e9-809f-d8f2ca3d9a0f/roslaunch-drone-Blade-1537.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://drone-Blade:45981/

SUMMARY
========

PARAMETERS
 * /crazyflieTypes/default/batteryVoltageWarning: 3.8
 * /crazyflieTypes/default/batteryVoltateCritical: 3.7
 * /crazyflieTypes/default/bigQuad: False
 * /crazyflieTypes/default/dynamicsConfiguration: 0
 * /crazyflieTypes/default/firmwareParams/ctrlMel/i_range_m_z: 1500
 * /crazyflieTypes/default/firmwareParams/ctrlMel/i_range_xy: 2.0
 * /crazyflieTypes/default/firmwareParams/ctrlMel/i_range_z: 0.4
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kR_xy: 70000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kR_z: 60000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kd_omega_rp: 200
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kd_xy: 0.2
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kd_z: 0.4
 * /crazyflieTypes/default/firmwareParams/ctrlMel/ki_m_z: 500
 * /crazyflieTypes/default/firmwareParams/ctrlMel/ki_xy: 0.05
 * /crazyflieTypes/default/firmwareParams/ctrlMel/ki_z: 0.05
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kp_xy: 0.4
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kp_z: 1.25
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kw_xy: 20000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/kw_z: 12000
 * /crazyflieTypes/default/firmwareParams/ctrlMel/mass: 0.032
 * /crazyflieTypes/default/firmwareParams/ctrlMel/massThrust: 132000
 * /crazyflieTypes/default/markerConfiguration: 0
 * /crazyflieTypes/large/batteryVoltageWarning: 11.4
 * /crazyflieTypes/large/batteryVoltateCritical: 11.1
 * /crazyflieTypes/large/bigQuad: True
 * /crazyflieTypes/large/dynamicsConfiguration: 0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/i_range_m_z: 1500
 * /crazyflieTypes/large/firmwareParams/ctrlMel/i_range_xy: 2.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/i_range_z: 4.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kR_xy: 30000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kR_z: 40000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kd_omega_rp: 100
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kd_xy: 3.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kd_z: 3.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/ki_m_z: 500
 * /crazyflieTypes/large/firmwareParams/ctrlMel/ki_xy: 1.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/ki_z: 2.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kp_xy: 8.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kp_z: 8.0
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kw_xy: 9000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/kw_z: 10000
 * /crazyflieTypes/large/firmwareParams/ctrlMel/mass: 0.38
 * /crazyflieTypes/large/firmwareParams/ctrlMel/massThrust: 5400
 * /crazyflieTypes/large/markerConfiguration: 2
 * /crazyflieTypes/medium/batteryVoltageWarning: 7.6
 * /crazyflieTypes/medium/batteryVoltateCritical: 7.4
 * /crazyflieTypes/medium/bigQuad: True
 * /crazyflieTypes/medium/dynamicsConfiguration: 0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/i_range_m_z: 1500
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/i_range_xy: 2.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/i_range_z: 2.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kR_xy: 50000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kR_z: 60000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kd_omega_rp: 200
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kd_xy: 1.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kd_z: 1.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/ki_m_z: 500
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/ki_xy: 0.1
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/ki_z: 0.5
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kp_xy: 2.0
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kp_z: 3
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kw_xy: 16000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/kw_z: 12000
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/mass: 0.11
 * /crazyflieTypes/medium/firmwareParams/ctrlMel/massThrust: 23000
 * /crazyflieTypes/medium/markerConfiguration: 1
 * /crazyflies: [{'initialPositio...
 * /crazyswarm_server/broadcast_address: FFE7E7E7E7
 * /crazyswarm_server/enable_logging: False
 * /crazyswarm_server/enable_parameters: True
 * /crazyswarm_server/firmware: crazyswarm
 * /crazyswarm_server/firmwareParams/commander/enHighLevel: 1
 * /crazyswarm_server/firmwareParams/ring/effect: 16
 * /crazyswarm_server/firmwareParams/ring/headlightEnable: 0
 * /crazyswarm_server/firmwareParams/ring/solidBlue: 255
 * /crazyswarm_server/firmwareParams/ring/solidGreen: 0
 * /crazyswarm_server/firmwareParams/ring/solidRed: 0
 * /crazyswarm_server/firmwareParams/stabilizer/controller: 2
 * /crazyswarm_server/firmwareParams/stabilizer/estimator: 3
 * /crazyswarm_server/force_no_cache: False
 * /crazyswarm_server/genericLogTopicFrequencies: [10]
 * /crazyswarm_server/genericLogTopic_log1_Variables: ['ctrlStat.edist']
 * /crazyswarm_server/genericLogTopics: ['log1']
 * /crazyswarm_server/motion_capture_type: vicon
 * /crazyswarm_server/object_tracking_type: libobjecttracker
 * /crazyswarm_server/print_latency: False
 * /crazyswarm_server/save_point_clouds: ~/pointCloud.ot
 * /crazyswarm_server/vicon_host_name: vicon
 * /crazyswarm_server/world_frame: /world
 * /crazyswarm_server/write_csvs: False
 * /crazyswarm_teleop/csv_file: /home/drone/crazy...
 * /crazyswarm_teleop/timescale: 0.8
 * /dynamicsConfigurations/0/maxFitnessScore: 0.001
 * /dynamicsConfigurations/0/maxPitch: 1.4
 * /dynamicsConfigurations/0/maxPitchRate: 20.0
 * /dynamicsConfigurations/0/maxRoll: 1.4
 * /dynamicsConfigurations/0/maxRollRate: 20.0
 * /dynamicsConfigurations/0/maxXVelocity: 2.0
 * /dynamicsConfigurations/0/maxYVelocity: 2.0
 * /dynamicsConfigurations/0/maxYawRate: 10.0
 * /dynamicsConfigurations/0/maxZVelocity: 3.0
 * /joy/dev: /dev/input/js0
 * /markerConfigurations/0/numPoints: 4
 * /markerConfigurations/0/offset: [0.0, -0.01, -0.04]
 * /markerConfigurations/0/points/0: [0.0177184, 0.013...
 * /markerConfigurations/0/points/1: [-0.0262914, 0.05...
 * /markerConfigurations/0/points/2: [-0.0328889, -0.0...
 * /markerConfigurations/0/points/3: [0.0431307, -0.03...
 * /markerConfigurations/1/numPoints: 4
 * /markerConfigurations/1/offset: [0.0, 0.0, -0.03]
 * /markerConfigurations/1/points/0: [-0.00896228, -0....
 * /markerConfigurations/1/points/1: [-0.0156318, 0.09...
 * /markerConfigurations/1/points/2: [0.0461693, -0.08...
 * /markerConfigurations/1/points/3: [-0.0789959, -0.0...
 * /markerConfigurations/2/numPoints: 4
 * /markerConfigurations/2/offset: [0.0, 0.0, -0.06]
 * /markerConfigurations/2/points/0: [0.0558163, -0.00...
 * /markerConfigurations/2/points/1: [-0.0113941, 0.00...
 * /markerConfigurations/2/points/2: [-0.0306277, 0.05...
 * /markerConfigurations/2/points/3: [0.0535816, -0.04...
 * /markerConfigurations/3/numPoints: 1
 * /markerConfigurations/3/offset: [0.0, -0.01, -0.04]
 * /markerConfigurations/3/points/0: [0.0177184, 0.013...
 * /numDynamicsConfigurations: 1
 * /numMarkerConfigurations: 4
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    crazyswarm_server (crazyswarm/crazyswarm_server)
    crazyswarm_teleop (crazyswarm/crazyswarm_teleop)
    joy (joy/joy_node)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[crazyswarm_server-1]: started with pid [1556]
process[joy-2]: started with pid [1667]
process[crazyswarm_teleop-3]: started with pid [1684]
process[rviz-4]: started with pid [1707]
[ INFO] [1553605576.061256283]: Wait for services...
[ INFO] [1553605576.062611655]: Manager ready.
[ INFO] [1553605576.073318118]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[ INFO] [1553605601.808298697]: Takeoff!




問題点5.1

エラーはなくボタンを押すとtake offと表示されるが、Crazyflieが飛ばない。

解決法5.1

yamlファイルで書き間違いがないか見る。(デバッグ途中)

6. その他

再帰性反射ボールの数を増やしてみる

結論は5,6個が適当。たくさんつけると逆に認識されにくくなった。5個を4本の足に一つずつつけ、残りの一つを中央上につけても、4個しか反応してくれない。
他の人のcrazyflieの再帰性反射ボールの付ける位置の例を見つけた。ボールは5,6個ぐらい。

7. まとめ

  • 以下のチュートリアルは完了。PID制御でwaypointを指定して飛行することができる。
    http://act.usc.edu/publications/Hoenig_Springer_ROS2017.pdf

  • 安定した制御をするには、自分でコントローラーのプログラムを書き換えるか(https://github.com/whoenig/crazyflie_ros) のコントローラーのプログラムを利用する。ただし、後者の場合は、利用方法は公式的な説明はなされていない。そのgithubのissueで該当プログラムについての議論とBitcrazeの公式ページを参照する必要がある。今後は実用化に向けて、PIDの値を調節したり、非線形コントローラーやカルマンフィルタを利用したりする必要があるだろう。

  • もしくは、新しくcrazyswarmを進めた方が良いかもしれない。(https://crazyswarm.readthedocs.io/en/latest/) 現在はこちらの方が開発が進んでいるらしい。whoenig氏はこちらへの移行を勧めている。

  • VIVEを使うのも一つの手

6
6
3

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
6
6