ROS2講座13 Ardupilotとmavrosで通信する

# 環境

この記事は以下の環境で動いています。

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-8250U |
| Ubuntu | 22.04 |
| ROS2 | Humble |

接続先のArdupilot

| 項目 |	値 |
|:--|:--|
| Board HW | Pixhawk (v2.4.8) |
| SW | Ardupilot |
| ver | Rover V4.1.5 (3ef2b3f4) |
# 概要

以前の講座で作った台車をコントールするArdupirotが動作しているPixhawkとROSをつないで動作させます。
Ardupilot-ROS間はUSBシリアルで通信します。このシリアル上でmavlinkプロトコルのデータを送受信します。malinkを解釈してROSと橋渡しするのがmavrosというROSノードです。

## ardupilot

Pixhawkという主にドローン向けのフライトコントローラー上で動くソフトウェアがArdupirotです。Ardupilotはドローンだけでなく、移動台車(Rover)用のモードもあります。
今回は以前の講座で作成したRover用の設定やモータードライバがつながっている物を用意します。

## mavlink

mavlinkでは特定の機能ごことにメッセージが決まっていて、例えばプロポのスティック値なら[RC_CHANNELS_SCALED](https://mavlink.io/en/messages/common.html#RC_CHANNELS_SCALED)というメッセージが定義されています。
移動台車で必要なメッセージはおおむねそろっています。

## mavros

mavrosはROSノードでmavlinkとROSの橋渡しをします。mavlinkを受信したら対応するROSメッセージを送信、ROSメッセージを自身したら対応するmavlinkを送信します。
coreの部分とpluginに分かれていて、plugin側で送受信の機能を持っています。後述しますが、pluginを自作して機能を追加することもできます。
また`mavros_extras`のROSパッケージで追加のpluginが入っています。

# 事前準備

## mavrosのインストール

```shell:mavrosのインストール
sudo apt install mavros mavros_extras
```

## udevのルール追加

pixhawkをUSBで接続すると`/dev/ttyACM0`として見えますが、これもほかのデバイスとの兼ね合いで名前が変わってしまいます。

```text:/etc/udev/rules.d/99-pixhawk.rules
SUBSYSTEM=="tty", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="5741", SYMLINK+="pixhawk"
```

これでPixhawkが`/dev/pixhawk`というリンクでみえます。

# mavrosの実行

```shell:mavrosの実行
ros2 launch mavros apm.launch
```

Pixhawkに正常に接続できれば以下のような出力が出ます。

```text:ardupilotの応答
[mavros_node-1] [INFO] [1690691419.661324988] [mavros.sys]: FCU: ArduRover V4.1.5 (3ef2b3f4)
[mavros_node-1] [INFO] [1690691419.661713891] [mavros.sys]: FCU: ChibiOS: 45395b6a
[mavros_node-1] [INFO] [1690691419.662114893] [mavros.sys]: FCU: Pixhawk1 002D0030 34355112 38393730
```

`ros2 topic list` をするとmavrosのメッセージが見えます。例えば`ros2 topic echo /mavros/imu/data --no-arr`とするとIMUの情報が取得できます。

# mavros経由で台車を制御する事前知識

ArdupilotからROSにプロポの情報を渡し、その情報から指令速度を生成して台車を駆動する例をのちに行います。まず先にその手順を実行するために必要な事前知識を説明します。

## GUIDEDモードでのARMの手順

モーターを駆動するためには以下の手順が必要です。

* SaftyスイッチのUnsafeへ切り替え
    * 通常はSaftyスイッチ(PixhawkにつながっているLED付きの押しスイッチ)が起動時でsafe(赤点滅)の状態です。これをunsafe(赤点灯)にする必要があります。方法としては以下の２通りがあります。
        * 起動後に毎回Saftyスイッチ長押しする
        * `BRD_SAFETY_DEFL`のパラメーターを0(DISABLE)にする
* GUIDEDモードへの切り替え
    * Ardupilotに外部から速度指令を送るには、ArdupilotをGUIDEDモードにする必要があります。
    * mavrosからはROSサービスでモード切替をすることが出来ます。
* ARMへの切り替え
    * Ardupilotの状態をDISARM->ARMに切り替えが必要です。
    * mavrosからはROSサービスでARMをすることが出来ますが、GUIDEDモードの時はglobal_positionの確定がarmの必須条件になっています。
        * GPSを受信できていれば問題ないですが、今回の構成では室内移動を主目的としてGPSモジュールを接続していません。その代わりにglobal_originの設定を行う必要があります。事前にROSトピック経由でglobal_originの設定が出来ます。

## StreamRate

Ardupilot側が出力する情報の送信レートにはArdupilotのパラメーターで決定します。このパラーメーターは(ポートの指定)_(内容)という名前をしています。`SR0_POSITION`はUSB経由のポートにおける自己位置系のメッセージに対応するポートで、これを10とすれば`LOCAL_POSITION`等の出力レートが10Hzになります。

| ポート | パラメーター名 |
|:-:|:-:|
| USB | SR0_* |
| Telemetry1 | SR1_* |
| Telemetry2 | SR2_* |
| Telemetry3(GPS) | SR3_* |
| Telemetry4 | SR4_* |
| Telemetry5 | SR5_* |

パラメーターと対応するメッセージの一覧については[開発者ガイド](https://ardupilot.org/dev/docs/mavlink-requesting-data.html)にもありますが一部情報が不完全です。最新の実装を知りたい場合は[GCS_MAVLINK_Parametersのコード](https://github.com/ArduPilot/ardupilot/blob/master/Rover/GCS_Mavlink.cpp#L409)の参照をお勧めします。

# RCコントローラーの操作で台車を動かす。

## ARMを行うソースコード

* `global_position/set_gp_origin`にpublish
* `set_mode`でGUIDEDモードに入れる。
* `cmd/arming`でARMに遷移

を順に行っていきます。

```c++:espresso_frame_device_lecture/src/rover_setup_node.cpp
#include <rclcpp/rclcpp.hpp>
#include <geographic_msgs/msg/geo_point_stamped.hpp>
#include <mavros_msgs/srv/command_bool.hpp>
#include <mavros_msgs/srv/set_mode.hpp>

using namespace std::chrono_literals;
using namespace std::placeholders;  // NOLINT

class RoverSetupNode : public rclcpp::Node {
 public:
  RoverSetupNode() : Node("rover_setup_node") {
    geo_origin_pub_ =
        this->create_publisher<geographic_msgs::msg::GeoPointStamped>(
            "/device/mavros/global_position/set_gp_origin", 1);
    mode_client_ =
        create_client<mavros_msgs::srv::SetMode>("/device/mavros/set_mode");
    arm_client_ = create_client<mavros_msgs::srv::CommandBool>(
        "/device/mavros/cmd/arming");
    timer_ = this->create_wall_timer(500ms,
                                     std::bind(&RoverSetupNode::onTimer, this));
  }

 protected:
  void onTimer(void) {
    switch (next_state_) {
      case State::INIT:
        // wait 1 step
        next_state_ = State::SEND_GEO_ORIGIN;
        break;
      case State::SEND_GEO_ORIGIN: {
        geographic_msgs::msg::GeoPointStamped geo_point{};
        geo_origin_pub_->publish(geo_point);
        RCLCPP_INFO(get_logger(), "geo_origin_pub");
        // wait 1 step
        next_state_ = State::SET_MODE;
        break;
      }
      /* fall through */
      case State::SET_MODE: {
        if (!mode_client_->service_is_ready()) {
          RCLCPP_WARN(get_logger(), "mode_request not connect\n");
          next_state_ = State::DONE;
          break;
        }
        auto mode_request =
            std::make_shared<mavros_msgs::srv::SetMode::Request>();
        mode_request->custom_mode = "GUIDED";
        auto mode_future = mode_client_->async_send_request(
            mode_request,
            std::bind(&RoverSetupNode::onSetMode, this, std::placeholders::_1));
        RCLCPP_INFO(get_logger(), "mode_request");
      }
      /* fall through */
      case State::WAIT_FOR_MODE:
        if (!set_mode_done_) {
          RCLCPP_INFO(get_logger(), "wait for mode_request");
          next_state_ = State::WAIT_FOR_MODE;
          break;
        }
      /* fall through */
      case State::DO_ARM: {
        if (!arm_client_->service_is_ready()) {
          RCLCPP_WARN(get_logger(), "arm_request not connect");
          next_state_ = State::DONE;
          break;
        }
        auto arm_request =
            std::make_shared<mavros_msgs::srv::CommandBool::Request>();
        arm_request->value = true;
        auto arm_future = arm_client_->async_send_request(
            arm_request,
            std::bind(&RoverSetupNode::onArm, this, std::placeholders::_1));
        RCLCPP_INFO(get_logger(), "arm_request");
      }
      /* fall through */
      case State::WAIT_FOR_ARM:
        if (!cmd_arm_done_) {
          RCLCPP_INFO(get_logger(), "wait for arm_request");
          next_state_ = State::WAIT_FOR_ARM;
          break;
        }
        RCLCPP_INFO(get_logger(), "ARM DOME");
      /* fall through */
      case State::DONE:
        next_state_ = State::DONE;
        break;
    }
  }

  void onSetMode(
      rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture future) {
    (void)future;
    set_mode_done_ = true;
  }

  void onArm(
      rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedFuture future) {
    (void)future;
    cmd_arm_done_ = true;
  }

 private:
  rclcpp::TimerBase::SharedPtr timer_{nullptr};
  rclcpp::Publisher<geographic_msgs::msg::GeoPointStamped>::SharedPtr
      geo_origin_pub_{nullptr};
  rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr mode_client_{nullptr};
  rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arm_client_{nullptr};

  enum class State {
    INIT,
    SEND_GEO_ORIGIN,
    SET_MODE,
    WAIT_FOR_MODE,
    DO_ARM,
    WAIT_FOR_ARM,
    DONE,
  } next_state_{State::INIT};
  bool set_mode_done_{false};
  bool cmd_arm_done_{false};
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto rover_setup = std::make_shared<RoverSetupNode>();
  rclcpp::spin(rover_setup);
  rclcpp::shutdown();
  return 0;
}
```

## Twistを生成するソースコード

```c++:espresso_frame_device_lecture/src/rc_to_cmd_vel_node.cpp
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <mavros_msgs/msg/rc_in.hpp>
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;
using namespace std::placeholders;  // NOLINT

class RcToCmdVelNode : public rclcpp::Node {
 public:
  RcToCmdVelNode() : Node("rc_to_cmd_vel_node") {
    twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(
        "/device/mavros/setpoint_velocity/cmd_vel", 1);
    joy_sub_ = this->create_subscription<mavros_msgs::msg::RCIn>(
        "/device/mavros/rc/in", 10,
        std::bind(&RcToCmdVelNode::onRCInReceived, this,
                  std::placeholders::_1));
  }

 protected:
  void onRCInReceived(const mavros_msgs::msg::RCIn::SharedPtr msg) {
    if (4 <= msg->channels.size()) {
      float value_1 = std::min(
          std::max((float)(msg->channels[1] - 1515) / 400.0f, -1.0f), 1.0f);
      float value_3 = std::min(
          std::max((float)(msg->channels[3] - 1515) / 400.0f, -1.0f), 1.0f);

      geometry_msgs::msg::TwistStamped twist;
      twist.header = msg->header;
      twist.twist.linear.x = -0.2f * value_1;
      twist.twist.angular.z = 1.0f * value_3;
      twist_pub_->publish(twist);
    }
  }

 private:
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_{
      nullptr};
  rclcpp::Subscription<mavros_msgs::msg::RCIn>::SharedPtr joy_sub_{nullptr};
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto joy_to_cmd_rate = std::make_shared<RcToCmdVelNode>();
  rclcpp::spin(joy_to_cmd_rate);
  rclcpp::shutdown();
  return 0;
}
```

* 今回のコードではRC送信機の左スティック(pitch軸、yaw軸)のみを使っています。右スティック上下(throtle軸)を動かすとArdupilotで`speed_nudge`という機能が働き速度が上書きされてしまいます。現状のArdupilotではこれをOFFにできないためにこの軸は実質使用できません

# 参考

* [mavlinkメッセージ一覧](https://mavlink.io/en/messages/common.html)
* [mavrosソースコード](https://github.com/mavlink/mavros)

# 目次ページへのリンク
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)