備忘録　ラズパイ5 ROS2 勉強再開 ⑤ ゲーム・コントローラ その2 ROS2で認識させる subX

　ROS2でXbox ワイヤレス コントローラを使います。ゲーム・パッドとも呼ばれているようです。
 　入力装置です。ROS2だとどういう扱いになるのでしょうか。
  　Xbox ワイヤレス コントローラは、USB、Bruuetoothの接続方法があります。入力ができると、例えば、カメさんturtlesimを動かして動作確認ができます。
 　 turtlesimは、Twistメッセージを受け取るという説明を見ます。そして、Xbox ワイヤレス コントローラはJoyメッセージらしいです。

### joyメッセージ
　どうも、検索すると、USB接続をするのが一般的なようです。ここでは、離れたところで使いたいので、Bruetoothを利用します。

 　前回、Xboxコントローラを見つけたとき、MACアドレスは68:6C:E6:43:52:0Cでした。
  　Xboxコントローラ接続開始ボタンを押せば、再接続するはずですが、いつまでたっても、上部の白色LEDは点滅したままです。つながると、点灯状態になるはずです。
   　bluetoothctlのpairコマンドでつなげました。表示がぐちゃぐちゃになっています。
    　勝手に再接続することもあり、実際の運用では、何らかの対策をしないといけないようです。
   
```
$ bluetoothctl
Waiting to connect to bluetoothd...[bluetooth]# Agent registerepair 
pair 68:6C:E6:43:52:0C

[CHG] Device 68:6C:E6:43:52:0C Connected: yes
[Xbox Wireless Controller]# [INFO] [1729472794.546684117] [joy_node]: Opened joystick: Xbox Series X Controller.  deadzone: 0.050000
[INFO] [1729472794.557685871] [joy_node]: Opened joystick: Xbox Series X Controller.  deadzone: 0.050000
[CHG] Device 68:6C:E6:43:52:0C ServicesResolved: yes

[Xbox Wireless Controller]#  <-CTRL-Z
[6]+  Stopped                 bluetoothctl

$ ls /dev/input/js*
/dev/input/js0
```

　参考文献　https://www.enthusiasticroboticist.com/blog/using-bluetooth-controller-with-ros-2-on-jetson-nano/

　nodeを起動します。
```
$ source /opt/ros/jazzy/setup.bash
$ ros2 run joy joy_node &
[2] 18596
yoshi@yoshi:~$ [INFO] [1729454313.181323308] [joy_node]: Opened joystick: Xbox Series X Controller.  deadzone: 0.050000
```
　Xboxコントローラを認識しているようですね。publishしている内容を見ます。

```x.cpp
$ ros2 topic echo /joy
header:
  stamp:
    sec: 1729473071
    nanosec: 628579945
  frame_id: joy
axes:
- -0.0  J1 左右方向 左:1, 右:-1 アナログ値
- -0.0  J1 上下方向 上:1, 下:-1 アナログ値
- -0.0  J3 左右方向 左:1, 右:-1 アナログ値
- -0.0  J3 上下方向 上:1, 下:-1 アナログ値
- 1.0   J5 1～-1 アナログ値
- 1.0   J4 1～-1 アナログ値
- 0.0   J2 左右方向 左:1, 右:-1 ディジタル値
- 0.0   J2 上下方向 上:1, 下:-1 ディジタル値
buttons:
- 0  A
- 0  B
- 0
- 0  X
- 0  Y
- 0
- 0  B
- 0
- 0  B4
- 0  B5
- 0  B1
- 0  B3
- 0
- 0
- 0
- 0  B2
```
　コントローラからの入力値も確認できました。 

 <img width="700" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/9927fa5e-7f65-717e-8fd1-f69c258b6d1c.png">



 <img width="700" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/e40c1f94-24e8-a222-db1d-a84018e24a92.png">


### joyプログラミング

　sensor_msgs/Joy Messageは、
```
Header header           # timestamp in the header is the time the data is received from the joystick
float32[] axes          # the axes measurements from a joystick
int32[] buttons         # the buttons measurements from a joystick 
```
　なんと大雑把な。
 　J1の左右、上下のデータは、axes[0]、axes[1]に入っていると思われます。

 　ROS2のプログラミングを中断する前の最後は、下記の記事です。

 https://qiita.com/jamjam/items/6c0b48747fd405989917

 　
　作業用ディレクトリはros2_wsです。その下にworksディレクトリを作って、
 　　CMakeLists.txt　と　package.xml
   　　srcディレクトリの下に、pubとsub

　今回作成するsubX.cppは、yoshi@yoshi:~/ros2_ws/works/src/subに入っています。
 
```
$ tree --charset=C

|-- CMakeLists.txt
|-- include
|   `-- works
|-- package.xml
`-- src
    |-- bme280
    |   |-- bme280_3.cpp
    |   |-- bme280.cpp
    |   |-- bme280F.cpp
    |   |-- read3data.cpp
    |   `-- readTemp.cpp
    |-- exec_sample
    |   `-- main.c
    |-- pub
    |   `-- pub.cpp
    `-- sub
        |-- sub.cpp
        |-- subF.cpp
        |-- subT.cpp
        |-- subX.cpp
        `-- subX.cpp.save
```

　``yoshi@yoshi:~/ros2_ws/works$``に移動し、ソースを編集します。 $ nano src/sub/subX.cpp 
  
```subX.cpp
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <iostream>
#include "sensor_msgs/msg/joy.hpp"

rclcpp::Node::SharedPtr g_node = nullptr;

void Xbox_callback(const sensor_msgs::msg::Joy::SharedPtr msg)
{
  RCLCPP_INFO(g_node->get_logger(), "%f  %f", msg->axes[0], msg->axes[1]);
}

int main(int argc, char **argv)
{
  std::cout << "start subscribe Xbox\n";

  rclcpp::init(argc, argv);
  g_node = rclcpp::Node::make_shared("Xbox_subscriber");

  auto subscriber = g_node->create_subscription<sensor_msgs::msg::Joy>
      ("/joy", 1, Xbox_callback);

  rclcpp::spin(g_node);

  g_node = nullptr;
  rclcpp::shutdown();
    return 0;
}
```

　CMakeLists.txtを編集します。package.xmlの変更はありません。

```CMakeLists.txt
GNU nano 7.2                              CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(works)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

add_executable(subX src/sub/subX.cpp)

ament_target_dependencies(subX rclcpp std_msgs sensor_msgs)

install(
  TARGETS subX
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()
```

　ビルドします。

```
yoshi@yoshi:~/ros2_ws/works$ source /opt/ros/jazzy/setup.bash
yoshi@yoshi:~/ros2_ws/works$ cmake -S . -B build
yoshi@yoshi:~/ros2_ws/works$ cd build
yoshi@yoshi:~/ros2_ws/works$ make
```

　publisherであるXboxのnodeを起動します。

```
yoshi@yoshi:~/ros2_ws/works$ ros2 run joy joy_node &
```

　コンパイルしたsubscriberを起動します。J1をぐりぐり動かしています。

```
yoshi@yoshi:~/ros2_ws/works/build$ ./subX

[INFO] [1729560268.749293181] [Xbox_subscriber]: 1.000000  -0.294830
[INFO] [1729560268.779092464] [Xbox_subscriber]: 1.000000  -0.329974
[INFO] [1729560268.808910340] [Xbox_subscriber]: 1.000000  -0.353746
[INFO] [1729560268.838716974] [Xbox_subscriber]: 1.000000  -0.361842
[INFO] [1729560268.868517850] [Xbox_subscriber]: 1.000000  -0.397565
[INFO] [1729560268.899392538] [Xbox_subscriber]: 0.992451  -0.435729
[INFO] [1729560268.929241729] [Xbox_subscriber]: 0.962382  -0.477330
[INFO] [1729560268.959054382] [Xbox_subscriber]: 0.937871  -0.519799
[INFO] [1729560268.988904739] [Xbox_subscriber]: 0.908155  -0.577656
[INFO] [1729560269.018739614] [Xbox_subscriber]: 0.618647  -0.883002
[INFO] [1729560269.048685563] [Xbox_subscriber]: 0.178634  -1.000000
[INFO] [1729560269.078469661] [Xbox_subscriber]: -0.230034  -1.000000
[INFO] [1729560269.109347535] [Xbox_subscriber]: -0.287569  -0.954865
[INFO] [1729560269.139191373] [Xbox_subscriber]: -0.352911  -0.643608
[INFO] [1729560269.169059396] [Xbox_subscriber]: -0.417129  -0.586298
[INFO] [1729560269.198898698] [Xbox_subscriber]: -0.470167  -0.511511
[INFO] [1729560269.228680018] [Xbox_subscriber]: -0.942015  -0.199997
[INFO] [1729560269.273381664] [Xbox_subscriber]: -1.000000  0.010044
[INFO] [1729560269.289366711] [Xbox_subscriber]: -1.000000  0.017047
[INFO] [1729560269.319157605] [Xbox_subscriber]: -1.000000  0.075964
```

　joyにかかわる記述に、Joyとjoyが混在しています。