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

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

Rover製作で作ってきたRoverとROS経由で接続します。

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

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

mavrosの使い方

インストール

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

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

設定ファイル

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ファイル

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つのノードについては後述

実行

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で設定しましょう。

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ノードです。

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コマンドを投げます。

ビルド

cd ~/catkin_ws
catkin build

参考

ROS mavros
ROS mavros-extra
Ardupilot SreamRate

目次ページへのリンク

ROS講座の目次へのリンク