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

# はじめに

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

https://qiita.com/kiwsdiv/items/6d1cf7ff10a3e32792e1

# 環境

- [ELECOM ‎JC-U4113SBK](https://www.elecom.co.jp/products/JC-U4113SBK.html) , [amazon](https://www.amazon.co.jp/%E3%82%A8%E3%83%AC%E3%82%B3%E3%83%A0-%E3%83%AF%E3%82%A4%E3%83%A4%E3%83%AC%E3%82%B9-%E3%82%B2%E3%83%BC%E3%83%A0%E3%83%91%E3%83%83%E3%83%89-Xinput-JC-U4113SBK/dp/B01MXY05W0/ref=pd_lpo_sccl_2/357-0346866-2324024?pd_rd_w=9Tu38&content-id=amzn1.sym.83be7c2b-52a5-4e81-8438-3eac7f5cf56f&pf_rd_p=83be7c2b-52a5-4e81-8438-3eac7f5cf56f&pf_rd_r=3CNH7V0X1RNM75TZW2VM&pd_rd_wg=wIt1d&pd_rd_r=f423c139-9afd-412e-8601-5a25a23f71b4&pd_rd_i=B01MXY05W0&th=1)

- RasberryPi 4B(Ubuntu 20.04.4 LTS)

| 項目 | バージョン |
|-- | -- | 
| Ubuntu|20.04|
|ROS|Noetic|
- [OpenCR](https://emanual.robotis.com/docs/en/parts/controller/opencr10_jp/#dynamixel-workbench)
- [Dynamixel PM42-010-S260-R](https://emanual.robotis.com/docs/en/dxl/p/pm42-010-s260-r/)
- 24V電源 (GSユアサ12VDCバッテリー*2)

# 参考

https://qiita.com/srs/items/9114bb3c27a148e0b855

https://qiita.com/seigot/items/80bb409ff2de351ee9fa

# やったこと
## joy用のパッケージのインストール
```bash
$ sudo apt-get install ros-noetic-joy
$ sudo apt-get install ros-noetic-joystick-drivers
```
joyスティックをPCに刺して以下の3つをターミナルで実行します。
1つめのターミナル
```bash
$ roscore
```
2つめのターミナル
```bash
$ rosrun joy joy_node
```
3つめのターミナル
```bash
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のコントローラを動作させます。
```bash
$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch
```

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

```bash
$ 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というパッケージを作成します。
```bash
$ 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用のディレクトリを作成します。

```bash
$ cd joy_teleope
$ mkdir src
$ mkdir launch
```

### basic_twist_publisher.cpp
`joy_teleope/src/basic_twist_publisher.cpp`を作成します。
参考記事のままです。[ROS講座07 joyプログラム](https://qiita.com/srs/items/9114bb3c27a148e0b855)

```bash
#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プログラム](https://qiita.com/srs/items/9114bb3c27a148e0b855)
```
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
<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>
```

### ビルド
```bash
$ cd ~/catkin_ws
$ catkin build
$ source ~/.bashrc
```

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

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

# さいごに

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