初めに
本記事では、unitree_ros2を用いて、Go2をキーボードから操作する２次開発方法についてご案内します。
環境
環境は、次のようなものを想定しています。
- 機種: Go2 R&D(Edu), R&D+(Edu+)（AIR/PROではないもの）
- Docking Station or 外部PC：ubuntu20/22, ROS2 foxy/humble
１．環境設定
を参考に、unitree_ros2の環境構築が完了しているものとします。
２．開発に必要な参考情報
-
サンプルコード:
sport_mode_ctrl.cpp – unitree_ros2 / github.com
-
sport_client一覧:
ros2_sport_client.h – unitree_ros2 / github.com
-
公式開発ドキュメント:
Sports Services Interface – Go2 SDK Development Guide
が参考になります。
ただしサンプルコードは、TrajectoryFollowの用法になり、今回使用したいMove関数は使っておりません。
３．実装例：cmd_vel_control
$ git clone https://github.com/TechShare-inc/go2_unitree_ros2 -b cmd_vel_controlcmd_vel_control.cppが新たに作成したプログラムになります。
このコードは、ROS2ノードを使用して/cmd_velトピックを購読し、Go2ロボットの動作を制御します。特に、cmdVelCallback関数で、キーボード操作から得た速度指令（/cmd_vel）をGo2ロボットの制御コマンドに変換しています
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "unitree_api/msg/request.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "common/ros2_sport_client.h"
using namespace std::placeholders;
class CmdVelToSportRequest : public rclcpp::Node
{
public:
CmdVelToSportRequest() : Node("cmd_vel_to_sport_request")
{
// コマンド速度トピックを購読
cmd_vel_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10, std::bind(&CmdVelToSportRequest::cmdVelCallback, this, _1));
// リクエスト送信用パブリッシャ
request_publisher_ = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 10);
}
private:
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel_msg)
{
// 受け取った/cmd_velメッセージを出力
RCLCPP_INFO(this->get_logger(), "Received cmd_vel: LinearX=%f, LinearY=%f, AngularZ=%f",
cmd_vel_msg->linear.x, cmd_vel_msg->linear.y, cmd_vel_msg->angular.z);
// /cmd_velメッセージをスポーツリクエストに変換
unitree_api::msg::Request req;
sport_client_.Move(req, cmd_vel_msg->linear.x, cmd_vel_msg->linear.y, cmd_vel_msg->angular.z);
// リクエストを送信
request_publisher_->publish(req);
// RCLCPP_INFO(this->get_logger(), "Published Request: LinearX=%f, LinearY=%f, AngularZ=%f",
// cmd_vel_msg->linear.x, cmd_vel_msg->linear.y, cmd_vel_msg->angular.z);
}
// 購読者
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber_;
// パブリッシャ
rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr request_publisher_;
// SportClientインスタンス
SportClient sport_client_;
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<CmdVelToSportRequest>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
４．実行
$ cd example_ws
$ colcon buildterminal 1:
$ source unitree_ros2/setup.sh
$ ros2 run unitree_ros2_examaple cmd_vel_controlterminal 2:
$ source unitree_ros2/setup.sh
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard実行後、terminal 1で動作ログが確認でき、terminal 2でキーボード操作を行います。
実行の様子：動画リンク
※動画は、Payloadに外部PC(Ubuntu22/humble)を搭載して行っているが、JetsonOrinからでも問題なく実行できる。
このように行うと、Go2をキーボードから操作することができます。
おわりに
本記事では、unitree_ros2を用いて、Go2をキーボードから操作する２次開発方法について、解説しました。
本記事を参考に、Move関数の使い方や他の制御方法を使用する際のヒントになれば幸いです。
この記事が少しでも役に立てたのなら幸いです。