えいあーるれいの技術日記

ROS2やM5 Stack、Ubuntuについて書いています

ROS2におけるjoy→Twist変換の簡単解説（joy2twist）

ROS2 ちょっとAIなロボット

PS5なロボット!? DualSenseをROS2で使う - えいあーる・れいの技術日記の続きです。（そういえば、全くAI要素ないなー）

　ROS2ではアクチュエータの回転や移動にはTwistメッセージをよく使うと思いますが、実際にコントローラからドライバを介して発せられるメッセージはJoyメッセージです。

　おそらく、ほとんどのモータ制御ノード（プログラム）はJoyメッセージを受信してその要素を使用してモータ制御に使うと思います。しかし、私は、マイコンボードを別途ROS2ノードにしてしまっているので、そうはいきません。

　そこで、Joy→Twistに変換できるプログラムを作成したので簡単に解説します。JoyメッセージをTwistに変換するプログラム自体はパッケージが公開されている(?)と思いますが、DualSenseには対応していないみたいなので個人的に作成したプログラムを用いて作り方を簡単に解説します。

なぜJoyそのままはダメなのか？

  1. キーボード入力や他のシミュレーションの入力がTwistだから
    • TurtlesimなどほとんどのシミュレーションがTwistを採用しています。
  2. Twistのほうがサイズが小さいから
    • Joyメッセージはint32型x16の配列とfloat32型x6の配列とHeader(4つのint32)の組み合わせとなりモノにもよりますが、マイコンには大きすぎます。Twistなら、Vector3(float32)x2=実質float32x6の配列だけになり、単純計算でも1受信あたり80byteの節約ができます。実際にはROSのメッセージは動的メモリの確保を行います。
    • 経験上、ESP32でJoyメッセージの送受信はできませんでした。（解決策があれば、ご意見頂けると幸いです）
  3. バージョン依存があるから
    • コントローラを変更するたびにマイコンのプログラムを変えるのは面倒です。当然ながら、長い間同じ仕様のほうが使いやすいです。

簡単な解説

　以下にプログラムを示します。ソースコードはGitHubにupしています。

github.com

#include "joy/ps_base.hpp"
#define JOY_VERSION PS5

#include <geometry_msgs/msg/twist.hpp>

#if JOY_VERSION == PS5
    #include "joy/ps5.hpp"
    using namespace ps5;
#elif JOY_VERSION == PS4
    #include "joy/ps4.hpp"
    using namespace ps4;
#elif JOY_VERSION == PS3
    #include "joy/ps3.hpp"
    using namespace ps3;
#endif

class example_joy:public rclcpp::Node, public ps
{
    public:
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist;
        geometry_msgs::msg::Twist send_data;

        void sub_joy_thread(const sensor_msgs::msg::Joy::SharedPtr msg)
        {
            get_data(msg);
            send_data.linear.x = -1*joy_left_x*127;
            send_data.linear.y = joy_left_y*127;
            send_data.angular.z = -1*joy_right_x*127;
            
            twist->publish(send_data);
        } 

        example_joy(const std::string name, const rclcpp::NodeOptions & options):Node(name, options)
        {
            using namespace std::chrono_literals;
            sub_joy = this->create_subscription<sensor_msgs::msg::Joy>("joy", 1, std::bind(&ps::sub_joy_thread, this, std::placeholders::_1));
            twist = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel",1);
        }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions options;
    auto node = std::make_shared<example_joy>("joy_test",options);
    
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

パーツに分けて解説します。

1. ファイルのインクルード

　joy/ps_base.hppは、自作のライブラリです。https://github.com/Ar-Ray-code/ps_ros2_commonで使用したものを流用しています。PS5を宣言していればps5.hppがインクルードされます。また、Twistのライブラリを導入しています。（rclcppはps_base.hppで宣言されています。）

#include "joy/ps_base.hpp"
#define JOY_VERSION PS5

#include <geometry_msgs/msg/twist.hpp>

#if JOY_VERSION == PS5
    #include "joy/ps5.hpp"
    using namespace ps5;
#elif JOY_VERSION == PS4
    #include "joy/ps4.hpp"
    using namespace ps4;
#elif JOY_VERSION == PS3
    #include "joy/ps3.hpp"
    using namespace ps3;
#endif

2. Classの宣言

　example_joyクラスを作成します。（名前は任意）

自作クラスには2つのクラス（rclcpp::Nodeとps）を継承します。

class example_joy:public rclcpp::Node, public ps
{
    public:
    
    ...
}

3. メンバ変数の宣言

　パブリッシャの送信用変数とパブリッシュ用スマートポインタを宣言します。（2つ）

// (class内のはなし)
        geometry_msgs::msg::Twist send_data;   // sub_joy_thread関数で宣言してもよい
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist;

4. Joyメッセージのコールバック関数

　受け取ったメッセージをTwistに変換します。

        void sub_joy_thread(const sensor_msgs::msg::Joy::SharedPtr msg)
        {
            get_data(msg);
            send_data.linear.x = -1*joy_left_x*127;
            send_data.linear.y = joy_left_y*127;
            send_data.angular.z = -1*joy_right_x*127;
            
            twist->publish(send_data);
        }

　JoyメッセージからTwistへの変換は写真の通りになっています左ジョイスティックは平行移動で、右ジョイスティックは旋回を表します。

f:id:Ray_ar:20210318231934p:plain

5. クラスの初期化（コンストラクタ）

　サブスクリプションとパブリッシャを宣言し、スマートポインタへの代入を行います。sub_joyはps_baseライブラリで宣言されています。

        example_joy(const std::string name, const rclcpp::NodeOptions & options):Node(name, options)
        {
            using namespace std::chrono_literals;
            sub_joy = this->create_subscription<sensor_msgs::msg::Joy>("joy", 1, std::bind(&ps::sub_joy_thread, this, std::placeholders::_1));
            twist = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel",1);
        }

6. main関数

　慣れると、テンプレートのコピペで済むようになります。auto node = make_shard<...>("..",options)は適宜変更してください。

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions options;
    auto node = std::make_shared<example_joy>("joy_test",options);
    
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

動作確認

  • joy2twistと同じようなcmakelist.txtを作成してコンパイルします。joy_nodeをインストールして動かします。うまく動けば、rqt_graphでつながります。引数は必要ありません。

f:id:Ray_ar:20210318231950p:plain