概要

初めに

ロボットを無線操作する方法は様々あると思いますが、せっかくROS2を勉強したので、ROS2で完結する通信方法を調べました。分かったことを記事にまとめます。
実戦投入はまだなので、参考程度に見てもらえればと思います。

大まかな手順

　最初に、ルーターを使って無線LANネットワークを構成し、PCとラズパイ間で無線通信を行います。次に、PC側の実装を行います。ジョイコンの入力をROS2で取得してtopicをpublishします。最後に、ラズパイ側でtopicをsubscribeしてマイコン側に指令を送ります。micro-ROSを用いることで、マイコンを一つのROS2 nodeとみなして通信環境を構成することができます。
※ラズパイのピンを使ってロボットを操作する場合はmicro-ROSの話題は読み飛ばしてください。

動作環境・バージョン

動作環境

Ubuntu22.04　：デュアルブートした実機PC
VScode　：PCとラズパイのコーディングは基本的にVScodeで行った
Raspberrypi 4 model B　
nucleo-f767zi　：現在弊部で一般的に用いられているマイコン
Archer C6　：無線LANルーター
logicool gamepad F310　：ジョイコントローラー

バージョン

ROS2 : humble　：現在弊部で推奨されているバージョン
micro-ROS：humble　：ROS2のバージョンと同じものを
cubeIDE ：1.13.2　：2023/12/20現在最新バージョン
raspiのOS: Ubuntu server 22.04 (64bit) 　：desktopでもいいが、軽いのでこちらを選択

コンテンツ

PCとラズパイの通信

　PCとラズパイ間の通信はROS2を場合、基本的には同じwifiに接続していれば可能です。が、念のためにpingで疎通確認を行いました。ifconfigでPCとラズパイのIPアドレスを調べて双方向にpingが通ることを確認しました。

　さらに今回は、ラズパイに無線LANだけでなく、有線LANも繋いでLidarと通信を行いました。その際、無線LANネットワークと有線LANネットワークが混線しないように固定IPアドレスを設定する必要がありました。
　LidarのIPアドレスはデフォルトでは192.168.0.10ですが、dhcpでラズパイに割り振られたＩＰアドレスが192.168.0.~だったためにサブネットが被ってしまっていることが問題です。
　固定IPはルーターの管理者画面から設定できます。使っているルーターによって管理者画面のUIは異なると思いますが概ね同じような設定ができるはずです。
私の使っているルーターの場合はブラウザの検索バーにhttp://tplinkwifi.netと入力することで、管理者画面に行くことができました。

アドレス予約の箇所から使いたい端末のMACアドレスとそれに割り当てたいIPアドレスを設定します。
サブネットがどうとか言うエラーがでた場合はネットワーク→LANからルーター自体のサブネットを設定することで解決できます。

PC側の実装

ジョイコンの入力をROS2で取得する

　ジョイコンの入力をROS2で取得するのは簡単で、ROS2に標準で搭載されているjoy_nodeを用いればよいです。ターミナルから起動する場合はros2 joy joy_nodeで起動できます。PCにジョイコンを差していればjoy nodeを起動した段階でsensor_msgs/msg/Joy型のメッセージがpublishされ始めます

モータの制御値をpublishする

　joy型のメッセージそのままではロボットを動かすのには不都合が大きいので、これをgeometry_msgs/msg/Twist型のメッセージに変換しましょう。

joy_transrate_node.cpp

#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
# include <bits/stdc++.h>
#include "sensor_msgs/msg/joy.hpp"

using namespace std::chrono_literals;

class JoyTransrateNode : public rclcpp::Node {
public:
    std::string type_;
    double lx_;
    double az_;

    JoyTransrateNode() : Node("joy_transrate_node") {

        //使用するパラメータの宣言
        declare_parameter("robot_type", "default");
        declare_parameter("max_lx", -1.0);
        declare_parameter("max_az", -1.0);

        //パラメータの取得
        type_ = get_parameter("robot_type").as_string();
        lx_ = get_parameter("max_lx").as_double();
        az_ = get_parameter("max_az").as_double();

        //パラメータの確認
        RCLCPP_INFO(this->get_logger(), "robot type:%s\r\n",type_.c_str());
        RCLCPP_INFO(this->get_logger(), "max lx:%f\r\n",lx_);
        RCLCPP_INFO(this->get_logger(), "max az:%f\r\n",az_);
  
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

        auto topic_callback = [this](const sensor_msgs::msg::Joy &msg) -> void {

            auto message = geometry_msgs::msg::Twist();

            //最大値に-1~1を乗算
            message.linear.x = lx_ * msg.axes[1];
            message.angular.z = az_ * msg.axes[0];

            //十字キーでの操作を上書き
            if(msg.axes[5] != 0){
                message.linear.x = lx_ * msg.axes[5];
            }
            if (msg.axes[4] != 0)
            {
                message.angular.z = az_ * msg.axes[4];
            }
            
            this->publisher_->publish(message);
        }; 

        subscription_ = this->create_subscription<sensor_msgs::msg::Joy>
                ("joy", 10, topic_callback);
    }
private:
    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<JoyTransrateNode>());
    rclcpp::shutdown();
    return 0;
}

コントローラーによってキーの対応関係が若干異なるそうなので、注意して下さい。
上のコードでは差動二輪ロボットを想定してキー入力をx方向の速度とz軸周りの角速度に変換しています。

必ずしも必要ではありませんが、今回はTwist型の速度司令をさらに各モータの制御値に変換するnodeを作成しました。

diff_drive_node.cpp

#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "drive_msgs/msg/diff_drive.hpp"
# include <bits/stdc++.h>

using namespace std::chrono_literals;

class DiffDriveNode : public rclcpp::Node {
public:

    std::string type_;
    std::double_t rad_;
    std::double_t dis_;

    //cmd_velからモータの制御値を計算
    void diffDrive(float cmd[2],float lx,float az){ //cmd[0]:右輪rps,cmd[1]:左輪rps,lx:m/s,az:rad/s

        cmd[0] = (lx + dis_*az/2)/(2.0*M_PI*rad_);
        cmd[1] = (lx - dis_*az/2)/(2.0*M_PI*rad_);
        
    }

    DiffDriveNode() : Node("diff_drive_node") {
        //使用するパラメータの宣言
        declare_parameter("robot_type", "default");
        declare_parameter("wheel_radious", -1.0);
        declare_parameter("wheel_distance", -1.0);

        //パラメータの取得
        type_ = get_parameter("robot_type").as_string();
        rad_ = get_parameter("wheel_radious").as_double();
        dis_ = get_parameter("wheel_distance").as_double();

        //パラメータの確認
        RCLCPP_INFO(this->get_logger(), "robot type:%s\r\n",type_.c_str());
        RCLCPP_INFO(this->get_logger(), "wheel radious:%f\r\n",rad_);
        RCLCPP_INFO(this->get_logger(), "wheel distance:%f\r\n",dis_);
        
        publisher_ = this->create_publisher<drive_msgs::msg::DiffDrive>("cmd_motor", 10);

        auto topic_callback = [this](const geometry_msgs::msg::Twist &msg) -> void {

            auto message = drive_msgs::msg::DiffDrive();
            float ans[] = {0,0}; //ans[0]:右輪rps,ans[1]:左輪rps
            diffDrive(ans, msg.linear.x, msg.angular.z);
            
            message.name = "differencial drive";
            message.m1 = ans[0];
            message.m2 = ans[1];

            this->publisher_->publish(message);
        }; 

        subscription_ = this->create_subscription<geometry_msgs::msg::Twist>
                ("cmd_vel", 10, topic_callback);
    }
private:
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
    rclcpp::Publisher<drive_msgs::msg::DiffDrive>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DiffDriveNode>());
    rclcpp::shutdown();
    return 0;
}

このコードでは各モータの速度目標値を計算しカスタムメッセージに格納しています。カスタムメッセージは以下のようなものを作成しました。

DiffDrive.msg

string name
float64 m1 #右輪rps
float64 m2 #左輪rps

ロボット側の実装

ラズパイでROS2 topicを中継

　次に、ラズパイでPCから送られてきたモータ制御値のtopicを中継します。今回はラズパイにubuntu serverを入れて、そこにROS2をインストールしているので普通のPCと同様にROS2を扱えます。ラズパイの詳しいセットアップ方法はこの記事を参考にしてください。

raspi_node.cpp


#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "drive_msgs/msg/diff_drive.hpp"

using namespace std::chrono_literals;

class RaspiNode : public rclcpp::Node {
public:
    RaspiNode() : Node("raspi_node") {
        
        publisher_ = this->create_publisher<drive_msgs::msg::DiffDrive>("/cmd_ras", 10);

        auto topic_callback = [this](const drive_msgs::msg::DiffDrive &msg) -> void {

            auto message = drive_msgs::msg::DiffDrive();

            message.name = msg.name;
            message.m1 = msg.m1;
            message.m2 = msg.m2;

            this->publisher_->publish(message);

            RCLCPP_INFO(this->get_logger(), "I'm hearing\r\n");
        }; 

        subscription_ = this->create_subscription<drive_msgs::msg::DiffDrive>("/cmd_motor", 10,topic_callback);

    }
private:
    rclcpp::Subscription<drive_msgs::msg::DiffDrive>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<drive_msgs::msg::DiffDrive>::SharedPtr publisher_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RaspiNode>());
    rclcpp::shutdown();
    return 0;
}

マイコンの実装

　ラズパイのGPIOピンを使う場合は必要ありませんが、マイコンを使いたい場合はさらにmicro-ROSを使ってモータ制御値のtopicをマイコン側で受け取れるようにする必要があります。今回用いたマイコンはstm32 f767ziで、cubeIDEを使いました。
以下、cubeIDEでの実装を紹介します。
micro-ROSの導入やカスタムメッセージの使い方などについてはこちらの記事を参考にさせて頂きました。
micro-ROSの準備ができたら、次にsubscriberを作成します。

//メッセージを受け取る変数の宣言
drive_msgs__msg__DiffDrive sub;

//subscriberの作成
   RCCHECK(rclc_subscription_init_default(
     &subscriber,
     &node,
     ROSIDL_GET_MSG_TYPE_SUPPORT(drive_msgs, msg, DiffDrive),
     "/cmd_ras"));
// エグゼキューターの作成。三番目の引数はextecuterに登録するコールバック関数の数。
RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));

// エグゼキューターにサービスを追加
RCCHECK(rclc_executor_add_subscription(
     &executor, &subscriber, &sub,
     &subscription_callback, ON_NEW_DATA));

subscribeのコールバック関数の中でモータを回す処理を書きます。今回は通信周りについての記事なので具体的なモータを回す実装については省略します。

void subscription_callback(const void * msgin)
{
	 // Cast received message to used type
	  const drive_msgs__msg__DiffDrive * sub = (const drive_msgs__msg__DiffDrive *)msgin;

      //デバッグ用の処理
	  char hearing[] = "I'm hearing from f7";
	  rosidl_runtime_c__String__assignn(&pub.data, hearing, sizeof(hearing));
	  RCSOFTCHECK(rcl_publish(&publisher, &pub, NULL));

	  run_motor(sub->m1,sub->m2); //適宜モータを回す処理を実装してください
}

動作確認

PC側で以下のnodeを起動します

joy_node
joy_transrate_node
diff_drive_node

ラズパイ側では

raspi_node　を起動し、

ros2 run micro_ros_agent micro_ros_agent serial -b 115200 --dev /dev/ttyACM0
でmicro-ROS　agentを起動して下さい。但し、使っているシリアルポートによってttyACM以下を書き換える必要があるかもしれません。
マイコンに給電して、適切なハードウェアを用意すればロボットを無線操作できるはずです。

終わりに

　今回はROS2とラズパイ、stmマイコンを使ってロボットを無線操作したときの実装を紹介しました。実際に使ってみた感想としては遅延が結構あって、これを改善しないと実戦投入は難しいかな、といったところでした。コントローラーの相性で遅延が発生することもあるそうなので、今後色々試してみて遅延を解消していきたいと思います。

参考文献