目的
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しているメッセージが分かる。
<launch>
<!-- マウスによる制御ノード -->
<node pkg="mouse_teleop" type="mouse_teleop.py" name="mouse_teleop">
<remap from="/mouse_vel" to="/rover_twist" />
</node>
</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で実装したが、これで一定速度になるかは不明。
#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ファイル更新
...(省略)
## 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>
<!-- 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>
<!-- 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]
---
...