ROS講座131 ArdupilotとROS経由で接続する

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

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要

[Rover製作](https://qiita.com/srs/items/5f44440afea0eb616b4a#rover%E8%A3%BD%E4%BD%9C)で作ってきたRoverとROS経由で接続します。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Board | pixhawk(v2.4.8) |
| Ardupilot | ArduRover V4.1.5 |

Ardupilotはmavlinkというプロトコルのメッセージをシリアル通信等でやり取りできます。このmavlinkを読み取るためにmavrosというROSパッケージを使用します。

# mavrosの使い方

## インストール

以下の2つのパッケージをインストールします。

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

## 設定ファイル

```yaml:hard_lecture/config/apm_config.yaml
# Common configuration for APM2 autopilot
#
# node:
startup_px4_usb_quirk: false

# sys_status & sys_time connection options
conn:
  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
  heartbeat_mav_type: "ONBOARD_CONTROLLER"
  timeout: 10.0          # hertbeat timeout in seconds
  timesync_rate: 0.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
  system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)

# sys_status
sys:
  min_voltage: 17.0   # diagnostics min voltage
  disable_diag: false # disable all sys_status diagnostics, except heartbeat

# sys_time
time:
  time_ref_source: "fcu"  # time_reference source
  timesync_mode: MAVLINK
  timesync_avg_alpha: 0.6 # timesync averaging factor

# command
cmd:
  use_comp_id_system_control: false # quirk for some old FCUs

# local_position
local_position:
  frame_id: "map"
  tf:
    send: true
    frame_id: "odom"
    child_frame_id: "base_link"
    send_fcu: false

# setpoint_velocity
setpoint_velocity:
  mav_frame: BODY_NED

# distance_sensor
distance_sensor:
  bottom:
    id: 0
    frame_id: "bottom_psd"
    field_of_view: 0.5
    send_tf: false
  back:
    id: 1
    frame_id: "back_sonar"
    field_of_view: 0.5
```

mavrosでは基本のROSノードにpluginを刺して機能を利用するという形になります。各プラグインについての設定のパラメーターが入っています。

* startup_px4_usb_quirk: PX4特有のクセを考慮するかというオプションです。今回はArdupilotを使うのでfalseにします。
* conn/timesync_rate: timesyncを有効にするといろいろ警告が出るので0.0で無効にします。
* local_position: tfを出力する設定をします。
* setpoint_velocity/mav_frame: cmd_velの値が何のフレームで適用するかを設定します。
* distance_sensor: センサーの数だけ設定が必要です。RNGFND1ならid:0、RNGFND2ならid:1になります。

## launchファイル

```xml:hard_lecture/launch/mavros.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="fcu_url" default="/dev/ttyACM0:57600" />

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

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

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

mavros_nodeはパラメーターで設定する項目が多いです。

* fcu_urlはpixhawkとつながっているシリアルポートを指定します。
* apm_pluginlists.yamlではロードするpluginの設定をします。PX4でしか使えないプラグインもあるので、それを除きます。
* 最後に起動している2つのノードについては後述

## 実行

```shell
roslaunch hard_lecture mavros.launch
```

例えば`rostopic echo /mavros/local_position/pose`等とするとLOCAL_POSITIONの値が表示されます。

# パラメーター設定を起動時に行うスクリプト

Ardupilotからテレメトリーで出すメッセージの出力レートはパラメーターで設定できます。
以下のrostopicの情報のレートが低いのでパラメーターを書き換えてレートを上げます。

|パラメーター名|Hz|対応するMAVLINK|対応するrostopic|
|:--|:--|:--|:--|
|SR0_POSITION|4->20|LOCAL_POSITION_NED|/mavros/local_position/odom|
|SR0_EXTRA1|4->20|ATTITUDE|/mavros/local_position/odom|
|SR0_RC_CHAN|4->20|RC_CHANNELS|/mavros/rc/in|

上記のmavros_nodeが動いている状態で、コンソールで次のようなコマンドを打つことでパラメーターを設定できます。`rosrun mavros mavparam set SR0_RC_CHAN 20`

ただlaunch時に設定したいので以下のようにpython scriptで設定しましょう。

```python:hard_lecture/scripts/mavparam.py
#!/usr/bin/env python3

from mavros_msgs.srv import *
import rospy

if __name__ == "__main__":
    rospy.init_node('mavpram_setter')

    rospy.sleep(10.0)
    rospy.wait_for_service('/mavros/param/pull')
    pull_param = rospy.ServiceProxy('/mavros/param/pull', ParamPull)
    res0 = pull_param(False)
    if not res0.success:
        rospy.logwarn('load mavparam fail')

    rospy.sleep(5.0)
    rospy.wait_for_service('/mavros/param/set')
    set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
    value = ParamSet
    value.integer = 20
    value.real = 0.0
    res1 = set_param('SR0_POSITION',value)
    res2 = set_param('SR0_EXTRA1',value)
    res3 = set_param('SR0_RC_CHAN',value)
    if not (res1.success and res2.success and res3.success):
        rospy.logwarn('set mavparam fail')

    rospy.loginfo("SR0_xxx will be effective from the next startup")
```

ただし`SR0_xxx`は再起動しないと適用されません。

# cmd_velでの操作で必要な操作

ROSからのcmd_velを受けて動かすにはArdupilotで以下の操作が必要です。

* Acroで動けるようにチューニングする
* GUIDEDモードに入れる
* Armする

これを行うのが以下のROSノードです。

```c++:hard_lecture/src/hard_mavros_bridge.cpp
#include "ros/ros.h"

#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <geographic_msgs/GeoPointStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/HomePosition.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Bool.h>

class MavrosBridge
{
public:
  MavrosBridge() : nh_(), pnh_("~")
  {
    joy_pub_ = nh_.advertise<sensor_msgs::Joy>("/mavros_bridge/joy", 10);
    set_gp_origin_pub_ = nh_.advertise<geographic_msgs::GeoPointStamped>("/mavros/global_position/set_gp_origin", 10);
    mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    arm_client_ = nh_.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    rc_sub_ = nh_.subscribe("/mavros/rc/in", 1, &MavrosBridge::rcCallback, this);
    activate_sub_ = nh_.subscribe("/mavros_bridge/activate", 1, &MavrosBridge::activateCallback, this);
    state_sub_ = nh_.subscribe("/mavros/state", 1, &MavrosBridge::stateCallback, this);
    hp_sub_ = nh_.subscribe("/mavros/home_position/home", 1, &MavrosBridge::hpCallback, this);
  }

  void rcCallback(mavros_msgs::RCIn msg)
  {
    sensor_msgs::Joy joy = convertRCtoJoy(msg);
    joy_pub_.publish(joy);
  }

  void activateCallback(std_msgs::Bool msg)
  {
    ROS_INFO("activate %u", msg.data);
    activate(msg.data);
  }

  void stateCallback(mavros_msgs::State msg)
  {
    last_state_ = msg;

    if (!initialized_)
    {
      initialSetup();
      initialized_ = true;
    }
  }

  void hpCallback(mavros_msgs::HomePosition msg)
  {
    hp_valid_ = true;
  }

  bool checkMove(void)
  {
    bool guided = last_state_.mode == "GUIDED";
    return last_state_.armed && guided && hp_valid_;
  }

  sensor_msgs::Joy convertRCtoJoy(const mavros_msgs::RCIn& msg)
  {
    sensor_msgs::Joy joy;
    joy.axes.resize(4);
    joy.buttons.resize(1);
    if (5 <= msg.channels.size())
    {
      float x = -((float)msg.channels[2] - 1510) / 410;
      float y = -((float)msg.channels[0] - 1510) / 410;
      float z = -((float)msg.channels[1] - 1510) / 410;
      float r = -((float)msg.channels[3] - 1510) / 410;
      joy.axes[0] = x;
      joy.axes[1] = y;
      joy.axes[2] = z;
      joy.axes[3] = r;

      auto getButton = [](const int b) {
        int output = 0;
        if (b < 1300)
          output = -1;
        else if (b < 1700)
          output = 0;
        else
          output = 1;
        return output;
      };
      joy.buttons[0] = getButton(msg.channels[4]);
    }
    return joy;
  }

  void initialSetup(void)
  {
    ROS_INFO("set GP origin");
    geographic_msgs::GeoPointStamped geo;
    set_gp_origin_pub_.publish(geo);
  }

  bool activate(bool activate)
  {
    ROS_INFO("set GUIDED");
    mavros_msgs::SetMode mode;
    mode.request.base_mode = 0;
    mode.request.custom_mode = "GUIDED";
    mode_client_.call(mode);

    ROS_INFO("set %s", activate ? "arm" : "disarm");
    mavros_msgs::CommandBool cmd;
    cmd.request.value = activate;
    arm_client_.call(cmd);

    return true;
  }

  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Publisher joy_pub_;
  ros::Subscriber rc_sub_;
  ros::Subscriber activate_sub_;
  ros::Subscriber state_sub_;
  ros::Subscriber hp_sub_;

  // for initial setup
  ros::Publisher set_gp_origin_pub_;
  ros::ServiceClient mode_client_;
  ros::ServiceClient arm_client_;

  mavros_msgs::State last_state_;
  bool hp_valid_{ false };
  bool initialized_{ false };
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "mavros_bridge");
  MavrosBridge mavros_bridge;
  ros::spin();
  return 0;
}
```

* 今回ArdupilotにGPSモジュールを刺していないのですが、global_positionが定まっていないとGUIDEDモードに入れません(もともとGUIDEDモードはGPS座標系で指定して移動するモードなため)。今回は`/mavros/global_position/set_gp_origin`を設定して回避します。
* あとはmode変更コマンドとarmコマンドを投げます。

## ビルド
```shell
cd ~/catkin_ws
catkin build
```

# 参考

[ROS mavros](http://wiki.ros.org/mavros)
[ROS mavros-extra](http://wiki.ros.org/mavros_extras)
[Ardupilot SreamRate](https://ardupilot.org/dev/docs/mavlink-requesting-data.html)

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