ROS講座07 joyプログラム

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ROSプログラムの一例を示します。`turtle_sim`をジョイスティックで操作すROSノードを作成します。今回は画面上の「亀」を動かすだけですが、「亀」を実際のロボットに置き換えればラジコンを行うことができます。

#joyスティックについて
今回はPS3コントローラーを使います。PS3のコントローラーはスティックの精度が高く、ほかのPC用のジョイスティックよりも使いやすいです。以下のaptリポジトリをインストールすればほかにドライバは必要なく、PS3コントローラーをUSBケーブルで接続するだけで使用できます。実はPS4のコントローラーも同様に使用できますが、キーの割り当ての順番が違うので注意が必要です。

## joy用のパッケージのインストール
```shell
sudo apt-get install ros-noetic-joy
sudo apt-get install ros-noetic-joystick-drivers
```

## 割り当て

![ps3割り当て.png](https://qiita-image-store.s3.amazonaws.com/0/254442/668be241-8394-3af6-3e84-ce6bd1e8a63e.png)

ボタン割り当て

| ボタン名 | 番号 |
|:-:|:-:|
| Select | 0 |
| L3(左軸押し込み)  | 1 |
| R3(右軸押し込み) | 2 |
| Start | 3 |
| 十字キー上 | 4 |
| 十字キー右 | 5 |
| 十字キー下 | 6 |
| 十字キー左 | 7 |
| L2 | 8 |
| R2 | 9 |
| L1 | 10 |
| R1 | 11 |
| △ | 12 |
| 〇 | 13 |
| × | 14 |
| □ | 15 |
| PS | 16 |

軸割り当て

| 軸名 | 割り当て | 正方向
|:-:|:-:|:-:|
| 左軸左右 | 0 | 左 |
| 左軸上下 | 1 | 上 |
| 右軸左右 | 2 | 左 |
| 右軸上下 | 3 | 上 |

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

```shell:1つめのターミナル
roscore
```

```shell:2つめのターミナル
rosrun joy joy_node
```

```shell:3つめのターミナル
rostopic echo /joy
```

3つ目のターミナルから以下のような出力を見ることができます。もしなにも出ていない場合はPS3コントローラーのPSボタンを押してください。

```text
header: 
  seq: 7364
  stamp: 
    secs: 1533999507
    nsecs: 676157619
  frame_id: ''
axes: [-0.0, -0.0, -0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, -0.0, -0.0, 0.07589953392744064, 0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---
```

# turtle_simのインストール
turtlesimはROSのチュートリアル用のROSパッケージです。その中でもturtle_simは実行すると以下のような「亀」のいる画面が表示されます。

```shell:turtle_simのインストール
sudo apt-get install ros-noetic-turtlesim 
```

```shell:turtle_simの実行(roscoreを別途実行すること)
rosrun turtlesim turtlesim_node 
```
![turtle_sim.png](https://qiita-image-store.s3.amazonaws.com/0/254442/d40df542-1354-d9bb-bfdc-be03fabedba5.png)

これをrosトピックを送ることで動かすことができます。この「亀」は`/turtle1/cmd_vel`というトピックに`geometry_msgs::Twist`型でROSトピックを投げると動きます。以下のコマンドは途中まで書いたら後はTabを押すと補完されるはずです。`Ctrl+C`を押すと止まります。

```shell:「亀」を動かす
rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 
```

#Twist送信ROSノードのソースコード
上のようにコマンドを送るのは手間なのでジョイスティックから動かせるようにします。joy nodeを使えばjoyスティックの入力を`/joy`というROSトピックをパブリッシュします。これのデータを変換して`Twist`を送信します。

## basic_twist_publisher
```c++:basic_lecture/src/basic_twist_publisher.cpp
#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
以下を追加します。

```cmake:basic_lecture/CMakeLists.txtに追加
add_executable(basic_twist_publisher src/basic_twist_publisher.cpp)

target_link_libraries(basic_twist_publisher
  ${catkin_LIBRARIES}
)
```

## launch
いろいろなROSノードを使うのでlaunchファイルにまとめます。

```xml:basic_lecture/launch/turtle.launch
<launch>
  <node name="joy_node" pkg="joy" type="joy_node" />
  <node name="basic_twist_publisher" pkg="basic_lecture" type="basic_twist_publisher">
    <remap from="cmd_vel" to="turtle1/cmd_vel"/>
  </node>
  <node name="turtlesim_node" pkg="turtlesim" type="turtlesim_node" />
</launch>
```

# ビルド
```shell
cd ~/catkin_ws
catkin build
```

#実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell:launch
roslaunch basic_lecture turtle.launch
```

ジョイスティックを動かすと画面上の「亀」が動きます。もしPS3を使っていて、「亀」が動かないときはPSボタンを押してください。

![basic_turtle.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/7d4d7a23-c786-aaa9-eff2-95e4f778c3b2.gif)

# 参考
[joyプログラム](http://ros-robot.blogspot.com/2010/01/ps3.html)
[PS3割り当て](http://wiki.ros.org/ps3joy)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)