Blog / R&D / ロボットIoT

MavrosでROSからArdupilotを接続する

geometry_msgs/Twist型トピックでシリアル経由でPixhawkに送って、操作指示が出せるらしい。
またROSで取得した自己位置推定のTFとかをArduPilotに送って建物内でもpixhawkで自動走行するなども可能らしい。
Ardupilotをguidedモードに入れるには、グルーバルポジションが必要なのでGPSがない場合はごまかす必要あるみたい。
rosとardupilotを繋ぐとGCSから操作するのめんどくさくなるんじゃと思ったがmavros自体にmavlink-routerの様な、updに変換してGCSに送る機能もついてるみたいなので、mission plannerから色々操作もできる。

構成例

事前セットアップ

使うもの

  • Pixhawk 2.4.8
  • USBケーブル
  • Ubuntuの入ったPC(今回はMacにParallels+ubuntu20)

    Pixhawk側のセットアップ

  • PixHawkのファームウェアをroverにしておく
  • preArmの条件やフェイルセーフを全て解除しておく
  • 動作確認のためにモーターを動かせるようにしておくと便利

ROS側の事前セットアップ

  • rosをインストールしておく
  • rosについてある程度理解しておく
  • 今回はnoeticを使用

mavrosのインストール

下記コマンドで必要パッケージをインストール

melodicを使用している場合はnoeticの部分をmelodicに書き換える

sudo apt install ros-noetic-mavros
sudo apt install ros-noetic-mavros-extras

パッケージの作成とビルド

この辺は丸々、この記事を参考にさせていただきました。https://qiita.com/srs/items/09d217c8b9f9e21d2f1d
細かいところで変更を加えています。

下記変更点

  • パッケージ名をmavros_testとした
  • c++ファイル名はmavros_bridge.cppとした
    必要なライブラリ

    std_msgs rospy roscpp mavros_msgs geographic_msgs sensor_msgs geometry_msgs
  • configファイルの最終行に下記を追加した
    こうすることでArdupilotに送られるmavrosからの指示をmission plannerからの指示に見せかけることができる

    # 追加設定
tgt_system: 1
tgt_component: 1
system_id: 255
component_id: 240
  • launchファイル名はmavros.launchとした
  • パッケージ名変更にあわせてlaunchファイル内も書き換えた

起動

事前準備

pixhawkとubuntuを接続します。接続した際に、/dev/ttyACM？が追加されていることを確認します。今回はACM0をつかうのですが、違うポートになっている際はlaunchを書き換えてください。なお、この作業は起動したときに毎回行う必要がありますが、めんどくさい時はudevルールを設定してください。※やり方は別記事を参照のこと。

権限の変更

sudo chmod 777 /dev/ttyACM0

起動コマンド

roscore

別のターミナルを開いて

roslaunch mavros_test mavros.launch

モードの変更とアーム。Disarmする際はfalseを送れば良い。

rostopic pub /mavros_bridge/activate std_msgs/Bool true

別の方法

最初に作成したc++ソースには、仮GlobalPointを送り、モード変更とArmが自動でできる機能がついている。手動でモードチェンジとarm/disarmすることもできる

Arm/Disarm

rosservice call /mavros/cmd/arming True
rosservice call /mavros/cmd/arming False
または
rosrun mavros mavsafety arm
rosrun mavros mavsafety disarm

モードチェンジ

rosservice call /mavros/set_mode "custom_mode: 'GUIDED'"
または
rosrun mavros mavsys mode -c MANUAL
または
rosrun mavros mavsys mode -b <ENUM VALUE>
ENUM Value カスタムモード
0 MANUAL
2 LEARNING
3 STEERING
4 HOLD
10 AUTO
11 RTL
15 GUIDED
16 INITIALISING

状態の確認

現在の状態を表示

rostopic echo /mavros/state
header: 
  seq: 151
  stamp: 
    secs: 1709079027
    nsecs: 233683329
  frame_id: ''
connected: True
armed: True
guided: True
manual_input: False
mode: "GUIDED"
system_status: 5
---

アームできていない時はarmedがfalseになっている。
アームできない原因はこんな感じで表示される。
今回はpixhawk内のアーム条件を厳しくしてたから、アームできなかった。
一度mission plannerを起動してフルパラメータリストから変更しよう。

GCS(missionplanner)との接続

pixhawkをubuntuにusb接続すると、mission plannnerに挿して動作確認ができない。
対策パターンとしては

  • pixhawkに繋いだPCからudpで別のmission plannerの入ったPCに送る
  • pixhawkのテレメトリ端子からUART接続する(もしくはテレメトリアンテナ接続)

の方法が使えると思う。

2個目については特に工夫点はいらない。
1個目についてはパラメータのgcs_urlを指定してあげると良いみたい。
launchファイルに追記しよう。
fcuはpixhawkのこと。

fcu_url:=serial:///dev/ttyACM0:57600 
gcs_url:=udp://pixhawkに繋いだコンピュータ:14555@missionplanner側のコンピュータ:14550

うちの環境ではこんな感じ。VPNに繋いでるので、遠隔でアクセスできる。

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="fcu_url" default="/dev/ttyACM0:57600" />
  <arg name="gcs_url" default="udp://:14555@192.168.30.10:14550" />

  <node pkg="mavros" type="mavros_node" name="mavros" clear_params="true" output="screen">
    <param name="fcu_url" value="$(arg fcu_url)" />
    <param name="gcs_url" value="$(arg gcs_url)" />

    <!-- load blacklist, config -->
    <rosparam command="load" file="$(find mavros)/launch/apm_pluginlists.yaml" />
    <rosparam command="load" file="$(find mavros_test)/config/apm_config.yaml" />
  </node>

  <node name="mavparam" pkg="mavros_test" type="mavparam.py" output="screen"/>
  <node name="mavros_bridge" pkg="mavros_test" type="mavros_bridge" output="screen"/>
</launch>

別の方法としてmavlink-routerも使用できる。
※別記事を参照してください。

動作確認

cmd_vel

pixhawkをguidedモードにしている時はcmd_velコマンドでロボットの操作ができるらしい。
が、実際に試したところ動かない。理由はわからないが、GPSが受信できる環境じゃないと動かなかった。

/mavros/setpoint_velocity/cmd_vel
/mavros/setpoint_velocity/cmd_vel_unstamped

にgeometry_msgs/Twist型またはgeometry_msgs/TwistStamped型のトピックを5Hz以上で送ってあげると動くらしい。

rc_override

これはrc信号を直接乗っ取る方法でmissionplannerのjoy機能と同等の使い方ができる。
モードはmanualにセットする。

/mavros/rc/override

にmavros_msgs/OverrideRCIn型のトピック(中身はただの16個のint型配列)を送ってやれば良い。

トピックを送る

なんらかのパッケージをいれるか自分で書けば良いのだが、とりあえず動作確認だけしたいという時はrostopic pubコマンドを使うと良い。

rostopic pub /mavros/setpoint_velocity/cmd_vel geometry_msgs/TwistStamped -r 10 "{header: auto, twist: {linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 2.0}}}"

出力確認

mission plannerのservo outputからみるのがわかりやすいが、下記コマンドでみると手っ取り早い。

rostopic echo /mavros/rc/out

参考

https://qiita.com/srs/items/09d217c8b9f9e21d2f1d
https://qiita.com/srs/items/1b14bdf0f1fd3c49cf49
http://wiki.ros.org/mavros/CustomModes
https://docs.ros.org/en/jade/api/mavros_msgs/html/srv/SetMode.html
https://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/CommandBool.html
https://github.com/mavlink/mavros/issues/1596
https://index.ros.org/p/mavros/
https://masoudir.github.io/mavros_tutorial/Chapter1_ArduRover_with_CLI/Step2_How_to_Arm_and_Disarm/
https://masoudir.github.io/mavros_tutorial/Chapter1_ArduRover_with_CLI/Step1_How_to_change_mode/