【Unitree Go2】 SDK,ROSから障害物検知ON/OFFにする方法

初めに
環境
１．障害物検知機能を使える条件
２．障害物回避の実装
おわりに

初めに

本記事では、デフォルト機能の障害物検知機能を２次開発で活用する方法についてご案内します。

環境

環境は、次のようなものを想定しています。

機種: Go2 R&D(Edu), R&D+(Edu+)（AIR/PROではないもの）
外部PC or Docking StaionのOrinを使用する

１．障害物検知機能を使える条件

結論から書くと、ROS2トピックには障害物検知のON/OFFを制御できるトピックはなく、SDKからのみ障害物検知のON/OFFが制御できます。

また、通常の速度指令の方法だと障害物検知をONにしていても、障害物検知は機能してくれません。
デフォルトで流れているDDSトピックrt/wirelesscontrollerが、joystick controllerと同等のことをソフトウェアから行うことができるものになっていますので、こちらを使えば障害物回避を使って制御できます。

２．障害物回避の実装

2-1. unitree_sdk2を入れる

外部PC or Docking Stationの適当な場所に、unitree_sdk2を入れます。
その後、READMEの手順通りにbuildを行い、正常に動作することを確認します。

$ cd /path/to/unitree_sdk2
$ sudo ./install.sh
$ mkdir build
$ cd build
$ cmake ..
$ make

2-2. 障害物回避のプログラムを作成する

障害物回避を使うためにはobstacle_avoid_client.hppをインクルードし、そのAPIを使う必要があります。

例えば弊社で下記のようなテストコードを作成しましたので、これをunitree_sdk2内の適当な場所に置きます。
その後、適切なCMakeLists.txtを作成し、コンパイルします。

#include <unitree/robot/go2/obstacles_avoid/obstacles_avoid_client.hpp>
#include <iostream>
#include <cstdlib> // 追加: atoi関数のため
 
int main(int argc, char* argv[]) // 引数を受け取るように変更
{
    /*
     * Initialize ChannelFactory
     */
    unitree::robot::ChannelFactory::Instance()->Init(0);
    unitree::robot::go2::ObstaclesAvoidClient oac;
 
    std::cout << std::boolalpha;
 
    oac.SetTimeout(5.0f);
    oac.Init();
 
    bool is_avoid;
    bool set_avoid = true; // デフォルト値は true に設定
 
    // コマンドライン引数から障害物回避の設定を読み取る
    if (argc > 1) { // 引数が与えられた場合
        set_avoid = atoi(argv[1]) != 0; // 0 以外の値なら true、0 なら false
    }
 
    int32_t ret = oac.Get(is_avoid);
    std::cout << "障害物回避機能の状態取得結果: 戻り値 = " << ret << ", 現在の設定 = " << (is_avoid ? "有効" : "無効") << std::endl;
 
    ret = oac.Set(set_avoid);
    std::cout << "障害物回避機能を" << (set_avoid ? "有効" : "無効") << "に設定: 戻り値 = " << ret << std::endl;
 
    ret = oac.Get(is_avoid);
    std::cout << "設定後の障害物回避機能の状態取得結果: 戻り値 = " << ret << ", 現在の設定 = " << (is_avoid ? "有効" : "無効") << std::endl;
 
    usleep(1000);
 
    return 0;
}

2-3. 実行する

上で作った障害物回避のプログラムを回すと、障害物検知のON/OFFを切り替えた場合、go2がしゃべってくれます。
そしてONの状態で、sdkのexampleのwirelessというものを動かすと障害物回避がONの状態でその場で旋回し続けます。

なお動きを止める場合はプログラムを止めた後に、joystickコントローラーを適当に動かす必要があります。

おわりに

Go2の障害物回避の２次開発利用方法を見てきました。
この記事が少しでも役に立てたのなら幸いです。

※誤字脱字、不明点等ございましたら遠慮なく問い合わせよろしくお願いいたします。

初めに

環境