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を使えるようにします。
全体構成は以下のようになります。
トピックの変換ノードは以下のリポジトリで公開しています。
次からはノードを作成する作業手順です。
パッケージの作成
以下のコマンドを実行し,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を使うことができました。