目的

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

動作環境

Raspberry Pi 4（Ubuntu 20.04）
(Github)メガローバー用のサンプルROSパッケージ

環境構築

基本的に取扱説明書通りにROSをインストール、ワークスペースの作成を行う。
ただし、取扱説明書でインストールしているROS MelodicはUbuntu 18.04用なので、今回はUbuntu 20.04用のROS Noeticをインストールする。
ROS wikiに全く同じ手順でROS Noeticのインストール方法が記載されている。

ROSメッセージ構成

ノードの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]に変換している。

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

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

取扱説明書[5.2 メガローバー用メッセージの仕様]にシミュレータ上のメガローバがpub/subしているメッセージ仕様が記載されている。
実機のメガローバがpub/subしているメッセージ仕様ではなく、記載もされていない。

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

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

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

mousectrl.launch

<launch>
  <!-- マウスによる制御ノード -->
  <node pkg="mouse_teleop" type="mouse_teleop.py" name="mouse_teleop">
    <remap from="/mouse_vel" to="/rover_twist" />
  </node>
</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に変換していることが分かる。
これは、取扱説明書[5.2 メガローバー用メッセージの仕様]、他機種の取扱説明書[6.5 メカナムローバー用メッセージの仕様]に記載されているメッセージ仕様に一致する。

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

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

joyconノード（`joycon.cpp`）

subscribe : /joy
publish : /rover_twist

pub_odomノード（`pub_odom.cpp`）

subscribe : /rover_odo
publish : /odom

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

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

ノード実装

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

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ファイル更新

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ノードをセットで起動する。

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に変換する。

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

実行（実機）

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

$ roslaunch megarover_samples speed_ctrl.launch

※rosserialノードの起動方法は、他機種の取扱説明書参照

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

取扱説明書[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]
---
...