ROS - メガローバー制御



# 目的

ROS理解、  
メガローバー用のサンプルROSパッケージを使用したメガローバー操作

# 動作環境

- Raspberry Pi 4（Ubuntu 20.04）
- [(Github)メガローバー用のサンプルROSパッケージ](https://github.com/vstoneofficial/megarover_samples)

# 環境構築

基本的に[取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/megarover_ros_simulator.pdf)通りにROSをインストール、ワークスペースの作成を行う。  
ただし、[取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/megarover_ros_simulator.pdf)でインストールしている`ROS Melodic`はUbuntu 18.04用なので、今回はUbuntu 20.04用の`ROS Noetic`をインストールする。  
[ROS wiki](http://wiki.ros.org/noetic/Installation/Ubuntu)に全く同じ手順で`ROS Noetic`のインストール方法が記載されている。  

# ROSメッセージ構成

![2023-02-19-11-44-22.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/695430/64d9187a-e840-1c62-748e-a7a6c5ca3945.png)

### ノードのpub/subメッセージ一覧

| ノード名 | pub/sub | メッセージ名 | 型 | 図内記号 |
| ---- | ---- | ---- | ---- | ---- |
|  joyノード  |  pub  | /joy | sensor_msgs/Joy | C |
|  joyconノード  |  sub  | /joy | sensor_msgs/Joy | C |
|  joyconノード  |  pub  |  /rover_twist | geometry_msgs/Twist | A-1 |
|  実機制御ノード  |  sub  |  /rover_twist  | geometry_msgs/Twist | A-1 |
|  実機制御ノード  |  pub  |  /rover_odo  | geometry_msgs/Twist | B-1 |
|  シミュレータ制御ノード  |  sub  |  /vmegarover/diff_drive_controller/cmd_vel  | geometry_msgs/Twist | A-2 |
|  シミュレータ制御ノード  |  pub  |  /vmegarover/diff_drive_controller/odom  | nav_msgs/Odometry | B-2 |
|  pub_odomノード  |  sub  |  /rover_odo  | geometry_msgs/Twist | B-1 |
|  pub_odomノード  |  pub  |  /odom  | nav_msgs/Odometry | B-2 |
|  速度制御ノード  |  sub  |  /odom  | nav_msgs/Odometry | B-2 |
|  速度制御ノード  |  pub  |  /rover_twist | geometry_msgs/Twist | A-1 |

※launchファイルにてメッセージ名は変換可能  

実機からpublishされるメッセージの情報は移動量[m/s]、旋回量[rad/s]で、  
pub_odomノードによって位置情報x,y[m]に変換している。  

以下にメッセージ構成を理解する過程を記載。  

## 取扱説明書からのメッセージ仕様理解

[取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/megarover_ros_simulator.pdf)[5.2 メガローバー用メッセージの仕様]にシミュレータ上のメガローバがpub/subしているメッセージ仕様が記載されている。  
実機のメガローバがpub/subしているメッセージ仕様ではなく、記載もされていない。  

実機のメッセージ仕様を探したところ、[他機種の取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/mecanumrover21_ROSmanual_20200819.pdf)[6.5 メカナムローバー用メッセージの仕様]に記載があった。（他機種だが同じはず）  
しかし、こちらにはシミュレータのメッセージ仕様が記載されていなかった。  

## launchファイルからのメッセージ仕様理解

[メガローバー用のサンプルROSパッケージ](https://github.com/vstoneofficial/megarover_samples)のlaunchフォルダ内に、シミュレータ用のlaunchファイル（`vmegarover_xxxxx.launch`）と実機用のlaunchファイル（`vmegarover_`なし）が用意されている。  
マウスでメガローバを操作するためのlaunchファイルのみ、シミュレータ用（`vmegarover_mousectrl.launch`）と実機用（`mousectrl.launch`）が両方用意されており、この差分からシミュレータ/実機それぞれがsubscribeしているメッセージが分かる。  

```launch:mousectrl.launch
<launch>
  <!-- マウスによる制御ノード -->
  <node pkg="mouse_teleop" type="mouse_teleop.py" name="mouse_teleop">
    <remap from="/mouse_vel" to="/rover_twist" />
  </node>
</launch>
```

```launch:vmegarover_mousectrl.launch
<launch>
  <!-- マウスによる制御ノード -->
  <node pkg="mouse_teleop" type="mouse_teleop.py" name="mouse_teleop">
    <remap from="/mouse_vel" to="/vmegarover/diff_drive_controller/cmd_vel" />
  </node>
</launch>
```

上記launchファイルから`mouse_teleop`ノードからpublishされているメッセージ`/mouse_vel`を、  
実機の場合はメッセージ`/rover_twist`に変換し、  
シミュレータの場合はメッセージ`/vmegarover/diff_drive_controller/cmd_vel`に変換していることが分かる。  
これは、[取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/megarover_ros_simulator.pdf)[5.2 メガローバー用メッセージの仕様]、[他機種の取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/mecanumrover21_ROSmanual_20200819.pdf)[6.5 メカナムローバー用メッセージの仕様]に記載されているメッセージ仕様に一致する。  

## ノードファイルからのメッセージ仕様理解

[メガローバー用のサンプルROSパッケージ](https://github.com/vstoneofficial/megarover_samples)のsrcフォルダ内にjoyconノード（`joycon.cpp`）とpub_odomノード（`pub_odom.cpp`）が用意されている。  
[取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/megarover_ros_simulator.pdf)[4.3 サンプルパッケージの内容について]には、  
「`megarover_samples にはメガローバー実機用に準備されたノードが含まれますが、シミュレータには関係ありません。`」としか記載はない。  
[他機種の取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/mecanumrover21_ROSmanual_20200819.pdf)[5.3 サンプルパッケージの内容について]には、  
「`「joycon」は、ゲームパッドなどのジョイスティックを使ってメカナムローバーを走行させられるノードです。「pub_odom」は、メカナムローバーのホイールエンコーダ値を基にオドメトリを出力するノードです。`」と記載がある。  
また、[6.5 メカナムローバー用メッセージの仕様]に「`本製品のサンプルパッケージに含まれるノード「pub_odom」は、“/rover_odo”をサブスクライブして、メカナムローバーの現在位置と姿勢を示すオドメトリ情報を計算して配信しています。`」と記載がある。  
上記説明と、ソースコードから以下のメッセージをpub/subしていることがわかる。  

### joyconノード（`joycon.cpp`）

subscribe : `/joy` <sensor_msgs/Joy>  
publish : `/rover_twist` <geometry_msgs/Twist>

### pub_odomノード（`pub_odom.cpp`）

subscribe : `/rover_odo` <geometry_msgs/Twist>  
publish : `/odom` <nav_msgs/Odometry>


# 速度制御用自作ノードの追加

一定速度でメガローバを制御するノードを作成、追加する。  

## ノード実装

一定速度で制御するためには常にメッセージをpublishし続ける必要がある。  
20Hzで実装したが、これで一定速度になるかは不明。  

```cpp:src/speed_ctrl.cpp
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>

double pos_x = 0.0;
double pos_y = 0.0;
double vel_x = 0.0;
double ang_z = 0.0;
int receive_flag = 0;

void odometryCallback(const nav_msgs::Odometry::ConstPtr& odometry){
  pos_x = odometry->pose.pose.position.x;
  pos_y = odometry->pose.pose.position.y;
  vel_x = odometry->twist.twist.linear.x;
  ang_z = odometry->twist.twist.angular.z;
  receive_flag = 1;
}

int main(int argc, char** argv){
  ros::init(argc, argv, "speed_ctrl");

  ros::NodeHandle n;
  ros::Publisher  twist_pub = n.advertise<geometry_msgs::Twist>("rover_twist", 50);
  ros::Subscriber odom_sub = n.subscribe("/odom", 100, odometryCallback);

  ros::Rate r(20); // 20Hz
  
  while(n.ok()){
    geometry_msgs::Twist twist;
    twist.linear.x = 1.0;  // 1.0 m/s
    twist.angular.z =0;    // 0 rad/s

    //publish the message
    twist_pub.publish(twist);  // 20Hzでpublishし続ける

    ros::spinOnce();
    r.sleep();
  }
}
```

## makeファイル更新

```txt:CMakeLists.txt
...(省略)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(mega_pub_odom src/pub_odom.cpp)
add_executable(mega_joycon   src/joycon.cpp)

### 追加 start
add_executable(mega_speed_ctrl   src/speed_ctrl.cpp)
### 追加 end

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(mega_pub_odom
  ${catkin_LIBRARIES}
)
target_link_libraries(mega_joycon 
  ${catkin_LIBRARIES}
)
### 追加 start
target_link_libraries(mega_speed_ctrl 
  ${catkin_LIBRARIES}
)
### 追加 end

set_target_properties(mega_pub_odom
                      PROPERTIES OUTPUT_NAME pub_odom
                      PREFIX "")

set_target_properties(mega_joycon
                      PROPERTIES OUTPUT_NAME joycon
                      PREFIX "")
### 追加 start
set_target_properties(mega_speed_ctrl
                      PROPERTIES OUTPUT_NAME speed_ctrl
                      PREFIX "")
### 追加 end

...(省略)
```

## launchファイル追加

### 実機用

実機からpublishされるメッセージをpub_odomノードによって位置情報x,y[m]に変換するため、  
pub_odomノードをセットで起動する。

```launch:speed_ctrl.launch
<launch>
  <!-- pub_odom -->
  <node pkg="megarover_samples" type="pub_odom" name="pub_odom" />

  <!-- speed_ctrl -->
  <node pkg="megarover_samples" type="speed_ctrl" name="speed_ctrl" respawn="true" />

</launch>
```

### シミュレータ用

実機用メッセージ`/rover_twist`をpublishする実装としているため、  
シミュレータ用メッセージ`/vmegarover/diff_drive_controller/cmd_vel`に変換する。  

```launch:vmegarover_speed_ctrl.launch
<launch>
  <!-- speed_ctrl -->
  <node pkg="megarover_samples" type="speed_ctrl" name="speed_ctrl" respawn="true">
    <remap from="/rover_twist" to="/vmegarover/diff_drive_controller/cmd_vel" />
  </node>

</launch>
```

# ビルド、実行

## ビルド

ワークスペース`catkin_ws`以下で以下コマンドでビルドを行う。  

```
$ catkin_make
```

## 実行（シミュレータ）

実行時はまずROSを起動し、その後、各種ノードを起動する。  
以下コマンドでROSを起動する。  

```
$ roscore
```

別のターミナルを起動し、ノードを起動する。（ノードごとに別ターミナルで起動する）  
ノード単体は`rosrun`コマンドで起動できる。  
`roslaunch`コマンドでlaunchファイルに定義した複数のノードを同時に起動できる。  

以下コマンドで、シミュレータを起動する。  

```
$ roslaunch megarover_samples vmegarover_with_empty_world.launch
```

※ `megarover_samples`はパッケージ名

以下コマンドで、自作した速度制御ノードをシミュレータ用に実行する。  

```
$ roslaunch megarover_samples vmegarover_speed_ctrl.launch
```

## 実行結果（シミュレータ）

Raspberry Piの性能上、シミュレータが重く、  
それが原因かは不明だが、想定した速度でメガローバが走行しなかった。  
実機では未確認。  

![192.168.11.7-_ubuntu_ubuntu_-VNC-Viewer-2023-02-19-13-56-48.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/695430/9ddb7e7e-5829-618b-768a-a15680d714b7.gif)

## 実行（実機）

実機の場合は、ROSと、rosserialノードを起動後に、
以下コマンドで、自作した速度制御ノードを実機用に実行する。

```
$ roslaunch megarover_samples speed_ctrl.launch
```

※rosserialノードの起動方法は、[他機種の取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/mecanumrover21_ROSmanual_20200819.pdf)参照

## ROSコマンド

### rostopic list

ROS上で起動しているノードがpub/subしているメッセージ一覧を表示する。  
以下、ROS、シミュレータを起動した状態で実行した結果。  

```
ubuntu@ubuntu:~$ rostopic list
/clock
/cmd_vel
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/odom
/rosout
/rosout_agg
/scan
/tf
/tf_static
/vmegarover/diff_drive_controller/cmd_vel
/vmegarover/diff_drive_controller/odom
/vmegarover/diff_drive_controller/parameter_descriptions
/vmegarover/diff_drive_controller/parameter_updates
/vmegarover/joint_states
```

[取扱説明書](https://www.vstone.co.jp/products/wheelrobot/pdf/megarover_ros_simulator.pdf)[5.2 メガローバー用メッセージの仕様]には、  
`/vmegarover/diff_drive_controller/odom`をpublishすると記載されているが、  
`/odom`（pub_odomノードがpublishするメッセージ）もpublishしているぽい。  

### rostopic echo

ROS上でpublishされた特定のメッセージをechoする。  
以下、ROS、シミュレータを起動した状態で`/odom`をechoした結果。  

```
ubuntu@ubuntu:~$ rostopic echo /odom
header: 
  seq: 3575
  stamp: 
    secs: 72
    nsecs: 163000000
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: -0.0002567864014272525
      y: 7.207869752057213e-10
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -1.1412589596017034e-05
      w: 0.9999999999348764
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
twist: 
  twist: 
    linear: 
      x: -3.727913643557739e-06
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -1.0335364439196792e-06
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
---
...
```