tshell_blog

ソフトウェアと車輪がついた乗り物のはなし

ROS1 Noetic とROS2 FoxyでUUV Simulatorを使う

UUV SimulatorはUUV(Unmanned Underwater Vehicle)のためのGazebo パッケージです。GitHubのmasterブランチは2020年に更新が止まっており,noeticに対応していませんが,このissueによるとnoeticに対応したソースを作ってくれている人がいます。
また,UUV SImulatorはROS1パッケージであり,UUVの舵やスラスタを動作させるトピック名もROS1仕様ですが,トピック名を変換するノードを記述してROS2と連携させてみます。

環境

  • ROS noetic
  • ROS2 foxy
  • Gazebo 11.1

ROSとROS2のインストール

以下の手順でnoeticとfoxyをインストールしておきます。

また,あとの行程でros1_bridgeを使うのでインストールしておきます。

$ sudo apt install ros-foxy-ros1-bridge

UUV Simulatorのクローンとビルド

以下のコマンドでUUV Simulatorのクローンとソースビルドを行います。UUV SImulatorだけではただ海の環境がGazeboに表示されるだけなのでLAUVやECA_A9といったUUVのモデルもインストールしておきます。
aptでインストールしたROSパッケージと同様に起動できるようにするため,-DCMAKE_INSTALL_PREFIX=/opt/ros/noeticをつけてcatkin_makeしています。

$ mkdir -p ~/uuv_sim_ws/src
$ cd ~/uuv_sim_ws/src
$ git clone -b noetic https://github.com/arturmiller/uuv_simulator.git
$ git clone https://github.com/uuvsimulator/rexrov2.git
$ git clone https://github.com/uuvsimulator/lauv_gazebo.git
$ git clone https://github.com/uuvsimulator/eca_a9.git
$ git clone https://github.com/uuvsimulator/desistek_saga.git
$ source /opt/ros/noetic/setup.bash
$ cd ~/uuv_sim_ws
$ catkin_make
$ sudo -s
# source /opt/ros/noetic/setup.bash
# catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install 

動作確認

コンソールを3つ開き,以下のコマンドを入力していきます。

1つ目のコンソールで以下を実行するとGazeboが起動し,海の環境がロードされます。

$ source /opt/ros/noetic/setup.bash
$ roslaunch uuv_gazebo_worlds ocean_waves.launch

2つ目のコンソールで以下のコマンドを実行し,lauvのモデルをロードします。

$ source /opt/ros/noetic/setup.bash
$ roslaunch lauv_description upload.launch mode:=default x:=0 y:=0 z:=-20

3つ目のコンソールで以下のコマンドを実行し,lauvを前進させます。

$ source /opt/ros/noetic/setup.bash
$ rostopic pub /lauv/thrusters/0/input uuv_gazebo_ros_plugins_msgs/FloatStamped '{header: {stamp: now}, data: -100}'

ROS2からUUV Simulatorを使う

上記の動作確認で,/lauv/thrusters/0/inputトピックにPublishしてスラスタを回転させていましたが,ROS2ではこのように途中に0だけの名前が入ったトピック名は許容されません。このため単純にros1_bridgeを実行しただけではROS2でUUV Simulatorを使うことができません。
そこで/lauv/thrusters/thruster0/inputをSubし,/lauv/thrusters/0/inputへPubするROS1ノードを作成し,ROS2からUUV Simulatorを使えるようにします。
全体構成は以下のようになります。

トピックの変換ノードは以下のリポジトリで公開しています。

github.com

次からはノードを作成する作業手順です。

パッケージの作成

以下のコマンドを実行し,uuv_bridgeという名前でROSパッケージを作成します。

$ source /opt/ros/melodic/setup.bash
$ cd ~/catkin_ws/src
$ catkin_create_pkg uuv_bridge rospy gazebo

CMakeLists.txtの編集

以下のように159行目付近のコメントアウトをもとに戻し,my_python_scriptをbridge.pyに置き換えます。

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
   scripts/bridge.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

ソースの記述

  • /lauv/fins/0/input〜/lauv/fins/3/input
  • /lauv/thrusters/0/input

へPublishするようなノードを作成します。
以下のソースをuuv_bridge_node.pyとしてscriptsディレクトリに保存します。
また,uuv_bridge_node.pyにはsudo chmod +x uuv_bridge_node.pyで実行権限を付加しておきます。

#!/usr/bin/env python
# -*- coding: utf-8 -*-                                                            
import rospy
from std_msgs.msg import Float64
from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped

# Subscriber用のコールバックを生成するクロージャー
def actuator_callback(inTopic, publisher):
    pub = publisher

    def callback(data):
        # rospy.loginfo(rospy.get_caller_id() + " I heard %f", data.data)

        pubdata = FloatStamped()
        pubdata.header.stamp = rospy.Time.now()
        pubdata.data = data.data
        
        pub.publish(pubdata)
    return callback

def converter(list):
    rospy.init_node('rosbridge_node', anonymous=True)

    # 変換リストからPublisher,SubScriberを設定
    for item in list:
        callback = actuator_callback(item[0], rospy.Publisher(item[1], FloatStamped, queue_size=10))
        rospy.Subscriber(item[0], Float64, callback)

    rospy.spin()

if __name__ == '__main__':
    convert_list = [
        # 変換元をSubscribeし,データをそのまま変換先へPublishする
        # 変換元,            変換先
        ('/lauv/fins/fin0/input', '/lauv/fins/0/input'),
        ('/lauv/fins/fin1/input', '/lauv/fins/1/input'),
        ('/lauv/fins/fin2/input', '/lauv/fins/2/input'),
        ('/lauv/fins/fin3/input', '/lauv/fins/3/input'),
        ('/lauv/thrusters/thruster0/input', '/lauv/thrusters/0/input'),
    ]
    converter(convert_list)

ビルド,インストール

以下のコマンドを実行してビルドします。

$ cd ~/catkin_ws
$ catkin_make

以下のコマンドでインストールします。

$ sudo -s
# source /opt/ros/noetic/setup.bash
# catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install 

動作確認

コンソールを5つ開き,以下のコマンドを入力していきます。

1つ目のコンソールで以下を実行するとGazeboが起動し,海の環境がロードされます。

$ source /opt/ros/noetic/setup.bash
$ roslaunch uuv_gazebo_worlds ocean_waves.launch

2つ目のコンソールで以下のコマンドを実行し,lauvのモデルをロードします。

$ source /opt/ros/noetic/setup.bash
$ roslaunch lauv_description upload.launch mode:=default x:=0 y:=0 z:=-20

3つ目のコンソールで以下のコマンドを実行し,先程作成したuuv_bridgeを起動します。

$ source /opt/ros/noetic/setup.bash
$ rosrun uuv_bridge uuv_bridge_node.py

4つ目のコンソールで以下のコマンドを実行し,ros1_bridgeを起動します。

$ source /opt/ros/foxy/setup.bash
$ ros2 run ros1_bridge dynamic_bridge

5つ目のコンソールで以下のコマンドを実行し,lauvのアクチュエータへ指令値をPubします。

$ source /opt/ros/foxy/setup.bash
$ ros2 topic pub /lauv/fins/fins0/input std_msgs/Float64 '{data: 0.5}'

また,以下のようにすることでlauvのセンサ値をROS2側で取得できます。

$ ros2 topic echo /lauv/gps

以上にROS2 FoxyからUUV Simulatorを使うことができました。