RTMとROSの連携コンポーネントの作成
eSEATでは、OpenRTM-aistのデータポートとROSのPubslisher/Subscriberの同時利用が可能です。そのため、RTCとROSノードのブリッジとして動作させることができます。
ここでは、RTCに対応したデバイスでROS対応のロボット（シミュレータ）の操作、またはその逆の動作をするSEATMLについて説明していきます。
また、ROSのPublisher/Subscriberのメッセージ型を変換するROSノードについても説明していきたいと思います。
TkJoystick(RTC)でTurtlesim(ROS)を動かす
まずは、Python版のOpenRTM-aistに含まれているTkinterを利用した仮想ジョイスティック TkJoystickとROSのサンプルの１つであるTurtlesimの連携を実装していきます。
準備（TkJoyStickとTurtlesimの動作確認）
TkJoystickは、OpenRTM-aistのサンプルに含まれており、通常、/usr/local/share/openrtm-1.1/python/example/TkJoyStickにインストールされています。
まずは、TkJoyStickComp.py を実行して動作確認をお願いいたします。
# cd /usr/local/share/openrtm-1.1/python/example/TkJoyStick
# python TkJoyStickComp.py &
# rtls /localhost
TkJoyStick0.rtc
#
下のWindowが表示され、TkJoyStick0.rtcが起動していればOKです。
次に、Turtlesimの動作確認を行います。Turtlesimのインストールと詳細は、ROSのチュートリアルを参照して下さい。
Turtlesimがインストールされていれば、下記のコマンドで、turtelsimのWindowsが表示されます。
# source /opt/ros/kinetic/setup.bash
# roscore &
# rosrun turtlesim turtlesim_node
以上で準備完了です。
SEATML（Rtc2Ros.seatml）の実装
それでは、TkJoyStickの出力をturlesimへの入力に変換するSEATMLを実装します。ファイル名を Rtc2Ros.seatml とします。まずは、雛形のコピーを行います。
# source /usr/local/eSEAT/setup.bash
# cd ~/work
# gen_seatml Rtc2Ros
次に、TkJoyStickからのデータを受信する入力データポートとturtlesimのturtle1への速度コマンドを出力するためのadaptorを定義します。
TkJoyStickのデータポートは、TkJoyStickが起動している状態で、rtcatコマンドで見てみます。
# rtcat -l /localhost/TkJoyStick0.rtc
TkJoyStick0.rtc Inactive
Category example
Description Sample component for MobileRobotCanvas component
Instance name TkJoyStick0
Type name TkJoyStick
Vendor Noriaki Ando and Shinji Kurihara
Version 1.0
Parent
Type Monolithic
+Extra properties
-Execution Context 0
State Running
Kind Periodic
Rate 1000.0
+Extra properties
-DataOutPort: pos
dataport.data_type IDL:RTC/TimedFloatSeq:1.0
dataport.dataflow_type push, pull
dataport.interface_type corba_cdr
dataport.subscription_type flush, new, periodic
port.port_type DataOutPort
-DataOutPort: vel
dataport.data_type IDL:RTC/TimedFloatSeq:1.0
dataport.dataflow_type push, pull
dataport.interface_type corba_cdr
dataport.subscription_type flush, new,
これからposとvelの２つありのデータ型は、両方共TimedFloatSeq であることがわかります。また、turtlesimの軌道状態で有効なTopicを調査します。
# rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
# rostopic info /turtle1/cmd_vel
Type: geometry_msgs/Twist
Publishers: None
Subscribers:
* /turtlesim (http://localhost:33735/)
上の結果から、turtlesimへのTopicは、/turtle1/cmd_velでメッセージ型は、geometry_msgs/Twistであることがわかります。
したがって、Rtc2Ros.seatmlのadaptorは、下のように定義できます。
<adaptor name="vel" type="rtcin" datatype="TimedFloatSeq" />
<adaptor name="turtle1/cmd_vel" type="ros_pub" datatype="geometry_msgs.Twist" />
adaptorを実装した時点で、正しく入力データポートとPublisherができているか確認します。まず、Rtc2Ros.seatmlでeSEATを起動します。
# eSEAT Rtc2Ros.seatml
次に各adaptorを確認します。
- 入力データポート入力データポートの確認は、rtcatを用います。
# rtcat -l /localhost/Rtc2Ros.rtc
Rtc2Ros.rtc Inactive
Category OpenHRI
Description eSEAT (Extened Simple Event Action Transfer)
Copyright (C) 2009-2014
Yosuke Matsusaka and Isao Hara
Intelligent Systems Research Institute,
National Institute of Advanced Industrial Science and Technology (AIST), Japan
All rights reserved.
Licensed under the MIT License (MIT)
http://www.opensource.org/licenses/MIT
Instance name Rtc2Ros
Type name eSEAT
Vendor AIST
Version 2.5
Parent
Type Monolithic
+Extra properties
-Execution Context 0
State Running
Kind Periodic
Rate 1.0
+Extra properties
-DataInPort: vel
dataport.data_type IDL:RTC/TimedFloatSeq:1.0
dataport.dataflow_type push, pull
dataport.interface_type corba_cdr
dataport.subscription_type Any
port.port_type DataInPort
これでTimedFloatSeq型の入力データポートvelができていることが確認できました。
- Publisherの確認
/turtle1/cmd_velのPublisherが正しくできているか確認します。確認には、rqt_graphを使って、Rtc2Rosのノードとturtlesimが /turtle1/cmd_velでつながっているかどうかを確認して下さい。
# rqt_graph
上記のコマンド入力後、下のようなWindowが出ればOKです。
これで、RTCからのデータ入力とROSのSubscriberへのメッセージ出力の準備ができましたので、コアロジックを実装します。
データ変換には、<script>要素を使います。これは、入力データの取得は、seat.get_in_data() で行い、データの出力は<script>要素のsendto属性を指定し、seat.set_resultメソッドで出力するデータをセットします。
出力データは、seat.newData()で行います。
RTCのTimedFloatSeqは、
struct TimedFloatSeq{
struct Time tm;
sequence<float> data;
}
となっており、ROSのgeometry_msgs/Twistは、
# rosmsg info geometry_msgs/Twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
となっています。従って、データ変換処理は下のように書くことができます。
<rule source="vel">
<script sendto="turtle1/cmd_vel">
data=seat.get_in_data()
cmd=seat.newData("turtle1/cmd_vel")
cmd.linear.x = data.data[1]/100.0
cmd.angular.z = -data.data[0]/100.0
seat.set_result(cmd)
</script>
</rule>
以上で、Rtc2Ros.seatmlの実装は終了です。最後に、動作確認をします。
添付ファイル：Rtc2Ros.seatml
Rtc2Rosの動作確認
動作確認をするには、roscore, TkJoyStick, turtlesim, eSEAT の起動と操作端末の５つのターミナルが必要です。
- ターミナル１(roscore)
# source /opt/ros/kinetic/setup.bash
# roscore
- ターミナル２(TkJoyStick)
# cd /usr/local/share/openrtm-1.1/example/TkJoyStick
# python TkJoyStickComp.py
- ターミナル３(turtlesim)
# source /opt/ros/kinetic/setup.bash
# rosrun turtlesim turtlesim_node
- ターミナル４（eSEAT）
# source /opt/ros/kinetic/setup.bash
# cd ~/work
# eSEAT Rtc2Ros.seatml
- ターミナル５（操作端末）
# rtcon /localhost/TkJoyStick0.rtc:pos /localhost/Rtc2Ros.rtc:vel
# rtcact /localhost/TkJoyStick0.rtc
# rtcact /localhost/Rtc2Ros.rtc
上記の操作の後、TkJoyStickを操作して下さい。turtlesimの亀が動けば動作完了です。
ROS joyでTkMobileSimulator（RTC）を動かす
上記では、RTCからROSノードへのデータ連携について述べました。次に、ROSノードからRTCを操作したいと思います。
準備
使うROSノードは、ゲームパッド用の joyを使い、OpenRTM-aistのサンプルであるTkMobileSimulatorを使います。TkMobileSimulatorは、Tkinterの拡張モジュールであるTixが必要ですので、下記のコマンドで予めインストールして下さい。
# sudo apt-get install tix
また、ROSノードのjoyのインストールは、下記のコマンドでインストールすることができます。
# sudo apt-get install ros-kinetic-joystick-drivers
joy_nodeの動作確認
まずは、joy_nodeの動作確認を行います。PCにJoystickまたはゲームパッドを接続し、下記のコマンドを入力します。
# source /opt/ros/kinetic/setup.bash
# roscore &
# rosrun joy joy_node
正しくjoy_nodeが動作すると、/dev/input/js0をオープンして下のようなメッセージ表示されます。
[ INFO] [1542751935.538645033]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
また、別のターミナルをオープンし、rostopicコマンドで /joy というトピックが表示されることを確認してください。
# source /opt/ros/kinetic/setup.bash
# rostopic list
/diagnostics
/joy
/rosout
/rosout_agg
あとは rostopic echo コマンドでJoystickの情報が取得できることを確認してください。以上で、joy_nodeの動作確認は終了です。
TkMobileSimulatorの動作確認
TkMobileSimulatorは、/usr/local/share/openrtm-1.1/example/python/MobileRobotCanvas の下にインストールされています。従って、下のコマンドでTkMobileSimulatorが起動することを確認してください。
# cd /usr/local/share/openrtm-1.1/example/python/MobileRobotCanvas
# python TkMobileRobotSimulator.py
この時、下のようなWindowが出ればOKです。
また、Createボタンを押下し、下のような五角形の移動ロボットが表示されれば、RTCも動作していると思います。rtlsコマンドなどでRTCが動作していることを確認してください。
# rtls /localhost
TkMobileRobotSimulator0.rtc
以上で、動作確認は終了です。
SEATML(Ros2Rtc.seatml)の実装
それでは、joy_nodeの出力をTkMobileRobotSimulator0.rtcへの入力に変換するSEATMLを実装します。ファイル名を Ros2Rtc.seatml とします。まずは、雛形のコピーを行います。
# source /usr/local/eSEAT/setup.bash
# cd ~/work
# gen_seatml Ros2Rtc
次に、joy_nodeからのデータを受信するSubscriberとTkMobileRobotSimulator0.rtcへの速度コマンドを出力するためのadaptorを定義します。
TkMobileRobotSimulatorのロボットは、TimedFloatSeq型のデータを受信し、[ 左の車輪の速度, 右の車輪の速度 ]として動作します。また、joy_nodeのPublisher '/joy'のメッセージ型は、sensor_msgs/Joy となっています。
したがって、Rso2Rtcのadaptorは、下のようになります。
<adaptor name="joy" type="ros_sub" datatype="sensor_msgs.Joy" />
<adaptor name="vel" type="rtcout" datatype="TimedFloatSeq" />
次に、データを変換のために、ros_subscriberのコールバック処理を<rule>要素で記述します。joy_nodeのメッセージ型である sensor_msgs/Joyは、rosmsgコマンドで調べると
# rosmsg info sensor_msgs/Joy
std_msgs/Header Header
uint32 seq
time stamp
string frame_id
float32[] axes
int32[] buttons
となっています。ジョイスティックのアナログスティックの出力は、axesの配列に格納されますので、コールバック処理は下のようになります。
<rule source="joy">
<script sendto="vel">
o_data=seat.newData("vel")
in_data=seat.get_in_data()
o_data.data.append(in_data.axes[0])
o_data.data.append(in_data.axes[1])
seat.set_result(o_data)
<script>
</rule>
この例では、アナログジョイスティックの出力は２次元であることを仮定していますが、ご使用のゲームパッドによっては、２つのアナログジョイスティックがあり、axesのインデックスは、操作しやすいものを選択してください。
Ros2Rtcの動作確認
最後に、動作確認をします。動作確認をするには、roscore, joy_node, TkMobileRobotSimulator, eSEAT の起動と操作端末の５つのターミナルが必要です。
- ターミナル１(roscore)
# source /opt/ros/kinetic/setup.bash
# roscore
- ターミナル２(joy_node)ジョイスティックをPCに接続して下記のコマンドを入力して下さい。
# source /opt/ros/kinetic/setup.bash
# rosrun joy joy_node
- ターミナル３(TkMobileRobotSimulator)
# cd /usr/local/share/openrtm-1.1/example/python/MobileRobotCanvas
# python TkMobileRobotSimulator.py
TkMobileRobotSimplatorの画面が表示されたら、"Create"ボタンで移動ロボットを生成して下さい。
- ターミナル４（eSEAT）
# source /opt/ros/kinetic/setup.bash
# cd ~/work
# eSEAT Ros2Rtc.seatml
- ターミナル５（操作端末）
# rtcon /localhost/Ros2Rtc.rtc:vel_out /localhost/TkMobileRobotSimulator0.rtc:vel
# rtcact /localhost/TkMobileRobotSimulator0.rtc
# rtcact /localhost/Ros2Rtc.rtc
上記の操作の後、ジョイスティックを操作して下さい。TkMobileRobotSimulatorの五角形の移動ロボットが動けば動作完了です。
ROS joyでTurtlesimを動かす（メッセージ型変換）
上記まで、OpenRTM-aistとROSとのeSEATによる連携について説明しました。最後に、ROS上で動作する様々なnode間のメッセージ型の変換例について説明します。
ROSでは、メッセージ型が簡単に作成することができ、初期に実装されたパッケージでは独自メッセージ型で作成されたものが多く存在します。そのようなnodeを再利用するには、独自パッケージを作成してnodeを再実装するのですが、ここではメッセージ型の変換nodeを作成して活用していきたいと思います。
このような活用例として、前述のOpenRTM-aistのRTCとの連携で利用したjoy_nodeとturtlesimとの接続を実現してみたいと思います。
joy_nodeではメッセージ型として sensor_msgs/Joy が利用されており、turtlesim_nodeでは geometry_msgs/Twistが利用されています。そこで、Joy2TwistというSEATMLを作成して eSEATをメッセージ変換nodeとして実行させてみます。
SEATML(Joy2Twist.seatml)の実装
それでは、joy_nodeの出力をturtlesim_nodeへの入力に変換するSEATMLを実装します。ファイル名を Joy2Twist.seatml とします。まずは、雛形のコピーを行います。
# source /usr/local/eSEAT/setup.bash
# cd ~/work
# gen_seatml Joy2Twist
次に、joy_nodeからのデータを受信するSubscriberとturtlesim_nodeへのデータを出力するためのPublisherを定義します。
<adaptor name="joy" type="ros_sub" datatype="sensor_msgs.Joy" />
<adaptor name="turtle1/cmd_vel" type="ros_pub" datatype="geometry_msgs.Twist" />
最後に、Ros2Rtcの時のように、Subscriberの処理を追加します。
ジョイスティックのアナログスティックの出力は、axesの配列に格納されますので、コールバック処理は下のようになります。
<rule source="joy">
<script sendto="turtle1/cmd_vel">
in_data=seat.get_in_data()
cmd=seat.newData("turtle1/cmd_vel")
cmd.linear.x = in_data.axes[1]
cmd.angular.z = in_data.axes[0]
seat.set_result(cmd)
<script>
</rule>
これで、Joy2Twist.seatmlの実装は終了です。
Joy2Twistの動作確認
最後に、動作確認をします。動作確認をするには、roscore, joy_node, turtlesim_node, eSEAT の４つのターミナルが必要です。
- ターミナル１(roscore)
# source /opt/ros/kinetic/setup.bash
# roscore
- ターミナル２(joy_node)ジョイスティックをPCに接続して下記のコマンドを入力して下さい。
# source /opt/ros/kinetic/setup.bash
# rosrun joy joy_node
- ターミナル３(turtlesim_node)
# source /opt/ros/kinetic/setup.bash
# rosrun turtlesim turtlesim_node
- ターミナル４（eSEAT）
# source /opt/ros/kinetic/setup.bash
# cd ~/work
# eSEAT_Node Joy2Twist.seatml
上記の操作の後、ジョイスティックを操作して下さい。turtlesim_nodeの亀が動けば動作完了です。
なお、ここでは eSEAT_Nodeコマンドを使いましたが、eSEATコマンドで実行させると起動時にはアクティベートされていませんので、rtshell等でアクティベートする必要があります。もし、eSEATでも起動時にアクティベートさせたい場合には、下のように<rule>要素に<onentry>要素を追加すればOKです。
<state name="main_state">
<onentry>
<script>
seat.activate()
</script>
</onentry>
...
...
</state>
