LoginSignup
0
1
@srs(project_srs)

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

Last updated at Posted at 2023-08-04

環境

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

項目 値
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というメッセージが定義されています。
移動台車で必要なメッセージはおおむねそろっています。

mavros

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

事前準備

mavrosのインストール

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

udevのルール追加

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

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

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

mavrosの実行

mavrosの実行
ros2 launch mavros apm.launch

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

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_*

パラメーターと対応するメッセージの一覧については開発者ガイドにもありますが一部情報が不完全です。最新の実装を知りたい場合はGCS_MAVLINK_Parametersのコードの参照をお勧めします。

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

ARMを行うソースコード

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

を順に行っていきます。

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を生成するソースコード

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にできないためにこの軸は実質使用できません

参考

目次ページへのリンク

ROS2講座の目次へのリンク

0
1
0

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
0
1