ゲームコントローラで作動二輪ロボットを制御するメモ

はじめに

DynamixelとOpenCRを使ったロボット制御のメモの続きです。
前回までで、キーボードで操作できるようになりましたが、今回はゲームコントローラで操作します。

環境

• ELECOM ‎JC-U4113SBK , amazon

• RasberryPi 4B(Ubuntu 20.04.4 LTS)

項目	バージョン
Ubuntu	20.04
ROS	Noetic

• OpenCR
• Dynamixel PM42-010-S260-R
• 24V電源 (GSユアサ12VDCバッテリー*2)

参考

やったこと

joy用のパッケージのインストール

$ sudo apt-get install ros-noetic-joy
$ sudo apt-get install ros-noetic-joystick-drivers

joyスティックをPCに刺して以下の3つをターミナルで実行します。
1つめのターミナル

$ roscore

2つめのターミナル

$ rosrun joy joy_node

3つめのターミナル

rostopic echo/joy
header:
  seq: 269
  stamp:
    secs: 1703140224
    nsecs: 944564797
  frame_id: "/dev/input/js0"
axes: [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---

コントローラ操作に応じて、値が変わっていることがわかります。

cmd_velトピックを送信してモータを動かす

ターミナルでDynamixel Workbenchのコントローラを動作させます。

$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

ここで、dynamixel_workbench/cmd_velトピックにgeometry_msgs::Twist型でROSトピックを投げることで動かすことができます。下記のコマンドでトピックをパブリッシュすることで、モータ(タイヤ)が動きました。

$ rostopic pub -r 10 /dynamixel_workbench/cmd_vel geometry_msgs/Twist "linear:
   x: 0.5
   y: 0.0
   z: 0.0
 angular:
   x: 0.0
   y: 0.0
   z: 0.0"

ゲームコントローラからモータを動かす

ゲームコントローラから動かせるようにします。joy nodeを使えばjoyスティックの入力を/joyというROSトピックをパブリッシュするので、これのデータを変換してTwistを送信します。

専用パッケージを作成する

ここでは、joy_teleopeというパッケージを作成します。

$ cd ~/catkin_ws/src
$ catkin_create_pkg joy_teleope roscpp std_msgs
Created file joy_teleope/package.xml
Created file joy_teleope/CMakeLists.txt
Created folder joy_teleope/include/joy_teleope
Created folder joy_teleope/src
Successfully created files in /home/ubuntu/catkin_ws/src/joy_teleope. Please adjust the values in package.xml.

srcとlaunch用のディレクトリを作成します。

$ cd joy_teleope
$ mkdir src
$ mkdir launch

basic_twist_publisher.cpp

joy_teleope/src/basic_twist_publisher.cppを作成します。
参考記事のままです。ROS講座07 joyプログラム

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>

class TwistPublisher{
public:
  TwistPublisher() : nh_(), pnh_("~") {
    cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    joy_sub_ = nh_.subscribe("joy", 10, &TwistPublisher::joyCallback, this);
    timer_ = nh_.createTimer(ros::Duration(0.1), &TwistPublisher::timerCallback, this);
  }

  void joyCallback(const sensor_msgs::Joy& joy_msg) {
    last_joy_ = joy_msg;
  }

  void timerCallback(const ros::TimerEvent& e) {
    int assign_x = 1;
    int assign_y = 0;
    int assign_z = 3;
    pnh_.getParam("assign_x", assign_x);
    pnh_.getParam("assign_y", assign_y);
    pnh_.getParam("assign_z", assign_z);

    float max_x = 0.5;
    float max_y = 0.5;
    float max_z = 1.5;
    pnh_.getParam("max_x", max_x);
    pnh_.getParam("max_y", max_y);
    pnh_.getParam("max_z", max_z);

    geometry_msgs::Twist cmd_vel;
    if(0 <= assign_x && assign_x < last_joy_.axes.size()){
      cmd_vel.linear.x = max_x * last_joy_.axes[assign_x];
    }
    if(0 <= assign_y && assign_y < last_joy_.axes.size()){
      cmd_vel.linear.y = max_y * last_joy_.axes[assign_y];
    }
    if(0 <= assign_z && assign_z < last_joy_.axes.size()){
      cmd_vel.angular.z = max_z * last_joy_.axes[assign_z];
    }
    cmd_pub_.publish(cmd_vel);
  }

  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Publisher cmd_pub_;
  ros::Subscriber joy_sub_;
  ros::Timer timer_;
  sensor_msgs::Joy last_joy_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "basic_twist_publisher");
  TwistPublisher twist_publisher;
  ros::spin();
  return 0;
}

CmakeList

CmakeListに以下を追加します。
参考記事のままです。ROS講座07 joyプログラム

add_executable(basic_twist_publisher src/basic_twist_publisher.cpp)

target_link_libraries(basic_twist_publisher
  ${catkin_LIBRARIES}
)

Launch

Launchファイルを作成します。
参考記事との差分は、Dynamixe_workbench_controllersのdynamixel_controllers.launch を起動するように指示しています。

<launch>
  <!-- dynamixel_workbench_controllersパッケージのdynamixel_controllers.launchを起動する -->
  <include file="$(find dynamixel_workbench_controllers)/launch/dynamixel_controllers.launch"/>
  <node name="joy_node" pkg="joy" type="joy_node" />
  <node name="basic_twist_publisher" pkg="joy_teleope" type="basic_twist_publisher">
    <remap from="cmd_vel" to="dynamixel_workbench/cmd_vel"/>
  </node>  
</launch>

ビルド

$ cd ~/catkin_ws
$ catkin build
$ source ~/.bashrc

実行

作成したパッケージのlaunchファイルを実行すると、ゲームコントローラでタイヤを制御することができます。

$ roslaunch joy_teleope dynamixel.launch

左スティックで前後、右スティックで回転するように動作します。

さいごに

本記事ではROS1の差動二輪のロボットをゲームコントローラを使って動せるようにしたメモを記載しました。