[Unitree Go2]ROS2を用いたプログラム作成（回転）

1. はじめに

Unitree Go2は二次開発可能な製品として弊社でも取り扱っているモデルではございますが、実際に二次開発を行うとして、知識のない方からすると障害が多いと思われます。本記事ではそんな方を対象に何となくわかった気になれる程度の知識の共有を目的としています。

こういった理由から今回はSDKの一部に追加する形でプログラムを作成いたします。

2. 環境

本記事ではUnitree Go2のdocking stationにリモートログインすることで実行しております。
その場合githubからファイルをダウンロードするのでインターネットに接続する環境が必要です。
メーカー記事では下記環境の機器を用いての実行となっております。
OS:Linux 20.04
（以下必須ではない）
また、作業環境として、なにかしらテキストエディタがあると編集しやすくなります。
弊社社員は”Visual Studio Code”を使用しています。
これに拡張機能として”C/C++”と”Remote SSH”を導入しています。

3. ROS2とは

ROS2（Robot Operating System 2）は、ロボットのソフトウェア開発を支援するフレームワークです。

• ノード（Node）: ロボットの機能を分割した小さなプログラム。それぞれが通信して協力し合います。
• トピック（Topic）: ノード間でメッセージをやり取りするためのチャネル。例えば、センサーからのデータを送信します。
• サービス（Service）: 要求と応答の形式で通信する仕組み。特定の処理を依頼し、その結果を受け取ります。
• アクション（Action）: 長時間かかるタスクのための通信形式。進行状況のフィードバックを受け取りながら実行されます。
• パッケージ（Package）: ノード、ライブラリ、設定ファイルなどの関連するファイルをまとめたもの。再利用や共有が容易になります。

4.プログラムの構成

ROS2の実行プログラムは大まかに以下の構成で作成されています

1.ヘッダーファイルのインクルード(プログラムの外から必要な情報を引用)
2.クラスの定義（決めなければならない値の設定）
3.メイン関数(実際にどういった動作をするか)

それぞれ車で走らせることに例えつつ解説します。

1.ヘッダーファイルのインクルード(プログラムの外から必要な情報を引用)

新しいプログラムを作成するときに1から細かいところまで定義すると非常に面倒です。その手間を省くためにある程度一般的な情報はここの部分で外部から引用します。

車で例えると、必要な部品を集めたりしている段階です。すでに作られたエンジンのような部品を作る必要がなくなります。

2.クラスの定義（決めなければならない値の設定）

実際にプログラムの動作を決める前にどういった要素をこのプログラムに入れるかを決めています。

車で例えると、「車」というのはどういった機能があるもので、どういった見た目をしたものかという仕様を定義する「設計図」のようなものです。

3.メイン関数(実際にどういった動作をするか)

この段階で動作をさせる機能は定義されているのでどういった動作をさせるか（GO2でいうと歩く向き、速さ）等を決定します。基本的にはROS２の初期化→指定動作の実行→ROS2の終了、といった流れで実行されます。

車で例えると、エンジンの始動→指定されたルートで走行→エンジンの停止、といった流れを決定します。

大体下図のイメージです。

５実践編

新規プログラムの追加

下記URLを基本的に参照してください。

unitreerobotics/unitree_ros2 (github.com)

ご使用の環境でフォルダを編集できる段階(Install Unitree ROS2 packageの直前)まで進めてください。

1.テキストエディタ等を用いて”unitree_ros2/example/src/src”フォルダ内に新しく”.cpp”ファイル(本記事ではtest_move.cpp)を作成する
2.下記コードをコピー&ペーストする

#include <unistd.h>
#include <cmath>

#include "rclcpp/rclcpp.hpp"
#include "unitree_go/msg/sport_mode_state.hpp"

#include "unitree_api/msg/request.hpp"
#include "common/ros2_sport_client.h"

using std::placeholders::_1;
// Create a soprt_request class for soprt commond request
class soprt_request : public rclcpp::Node
{
public:
    soprt_request() : Node("req_sender")
    {
        // the req_puber is set to subscribe "/api/sport/request" topic with dt
        req_puber = this->create_publisher<unitree_api::msg::request>("/api/sport/request", 10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(int(dt * 1000)), std::bind(&soprt_request::timer_callback, this));
    };

private:
    void timer_callback()
    {
            // Get request messages corresponding to high-level motion commands
            sport_req.Move(req, 0.0, 0.0, 0.2);
            // Publish request messages
            req_puber->publish(req);
    };

    rclcpp::TimerBase::SharedPtr timer_; // ROS2 timer
    rclcpp::Publisher<unitree_api::msg::request>::SharedPtr req_puber;

    unitree_api::msg::Request req; // Unitree Go2 ROS2 request message
    SportClient sport_req;

    double dt = 0.002; //control time step
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv); // Initialize rclcpp

    rclcpp::spin(std::make_shared<soprt_request>()); //Run ROS2 node

    rclcpp::shutdown();
    return 0;
}

上記コードにある程度説明は入れてありますが、一部詳細に解説します。

ヘッダーファイルのインクルード

下記画像部分が該当します。

こちらの部分に関しては前述の通り、必要な情報を引用している部分です。
より高度なことをする場合はさらに追加で必要になりますが、今回のように運動させるだけならこれで大丈夫です。

クラスの定義

ノードの作成

どういった構成のノードにするかがここで決定されています。

class soprt_request : public rclcpp::Node: ROS2のノードとして機能するための基礎を提供します。

soprt_request() : Node(“req_sender”): このノードの名前をreq_senderと指定しています。

req_puber: メッセージを発行するためのパブリッシャーを作成します。ここでは、”/api/sport/request”というトピックにメッセージを送信します。今回の構成だとsport_clientというノードがこのメッセージを受信して動作するようになっています。

timer_: タイマーを作成し、dtで設定された間隔ごとにtimer_callback関数を呼び出します。dtは制御時間ステップで、この場合0.002秒（2ミリ秒）です。

timer_callback(): この関数は、タイマーが設定された時間ごとに呼び出されます。ここでは、sport_req.Moveを使って動作コマンドを設定し、それをreq_puber->publish(req)で発行します。今回は

sport_req.Move(req, 0.0, 0.0, 0.2)

としております。数字部分は左からx方向、y方向、yaw方向にどれだけ動かすかを指定します。
違う動作をさせたい場合はこの部分を変更し、buildすれば動作させられます。

他にどういったコマンドが使用できるかは”unitree_ros2/example/src/src/common/ros2_sport_client.cpp”を参照してください。

CMakeists.txtの編集

今のままだと新しく作ったプログラムがbuildされないので”CMkaeList.txt”というファイルを編集します。このファイルがどのプログラムをbuildするかを決定します。

この画像を参考に43.51.62行目を追加してください。（必要に応じてプログラム名の部分を変更してください。）

ROS2パッケージのインストール

ファイルの用意ができたのでここからはファイルを実行できるようにします。

sudo apt install ros-foxy-rmw-cyclonedds-cpp
sudo apt install ros-foxy-rosidl-generator-dds-idl
sudo apt install gedit

このコマンドを打つことでパッケージのインストールに必要な機能をインストールします。

sudo apt install gedit
sudo gedit ~/.bashrc

すでにROS２の環境がある場合は下記コマンドにてROS２の環境を勝手に起動させないようにしてください。

sudo apt install gedit
sudo gedit ~/.bashrc

~はROS2をインストールしたディレクトリを指しています。上記コマンドでテキスト編集画面になりますので”

source /opt/ros/foxy/setup.bash

の部分を探し、”#”をつけることでコメントアウトさせてください。

cyclone ddsのコンパイル

Unitree　ROS2をしようするためにcyclone ddsというものをコンパイルする必要があります、コンパイルとは人がわかりやすいように書かれたコードを機械がわかりやすく翻訳するみたいなことです。下記コマンドを順に実行してください。

cd ~/unitree_ros2/cyclonedds_ws/src
git clone https://github.com/ros2/rmw_cyclonedds -b foxy
git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x 
cd ..
colcon build --packages-select cyclonedds