ros_arduino_bridgeを使ってパンチルトカメラを制御

以前の「ArduinoでRCサーボを制御」では、ros_arduino_bridgeパッケージを使ってArduinoに接続したRCサーボを制御する方法を紹介しました。

前回、カスタマイズした部分は、Arduinoのマイコンに書き込むスケッチだけでした。ArduinoをRCサーボのコントローラとして機能するように設定しました。 一方、ROS側は未だ何のカスタマイズも行っていません。

今回は、ros_arduino_bridgeを使って2つのRCサーボをもったパンチルトカメラを制御します。ros_arduino_bridgeをパンチルトカメラで使うためのptcam_arduinoパッケージを作成していきます。ptcam_arduinoパッケージには、launchファイルやパラメータの設定ファイル、また動作確認用のノードなどを入れていくことにします。

yamlパラメータファイルを用意する

ノードのパラメータは、launchファイルに記述することで設定できますが、パラメータの数が多いときにはlaunchファイルが見にくくなります。

そこで、yamlファイルにパラメータをまとめて記述します。launchファイルでyamlファイルを指定すれば、必要なパラメータをまとめて設定できます。

このようにyamlファイルを利用すれば、複数のlaunchファイルでパラメータを共有することもできるので、パラメータの管理が楽になります。

yamlファイルをptcam_arduino/config/ptcam_arduino_params.yamlとして作成します。その一部を以下に示します。

port: /dev/ttyUSB0
baud: 57600
use_base_controller: False
base_controller_rate: 10

sensors: {
  onboard_led:           {pin: 13, type: Digital, rate: 5, direction: output}
}

joints: {
    pan:  {pin: 3, init_position: 0, init_speed: 20, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False},
    tilt: {pin: 4, init_position: 0, init_speed: 20, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False}
}
		

このようにyamlでは、階層をもってパラメータを記述できます。

例えば、Pythonで書かれたノード内では、rospy.get_param("~joints/pan/init_speed", 10)とすれば、パラメータ/joints/pan/init_speedの値20を得ることができます。

launchファイルでyamlファイルを指定する

起動用launchファイルをptcam_arduino/launch/ptcam_manctrl.launcとして作成します。


<launch>
   <node name="ptcam" pkg="ros_arduino_python" type="arduino_node.py" output="screen" clear_params="true">
      <rosparam file="$(find ptcam_arduino)/config/ptcam_arduino_params.yaml" command="load" />
   </node>
</launch>
		

ここで、ros_arduino_bridgeのノード"arduino_node.py"をノード名"ptcam"で起動しますが、そのときrosparamでyamlファイルを指定しています。

これにより、このノードを起動するときにyamlファイルが読み込まれ、そのパラメータが使用されます。

以上の手順により、ros_arduino_bridgeがパンチルトカメラのコントローラとして使えるようになります。

いよいよ実機で試してみます。前回Arduinoにスケッチを書き込みましたが、これをPCに接続してlaunchファイルを実行します。 rostopic listにより、/pan/commandや/tilt/commandなどのトピックが現れていることが確認できます。

/pan/commandトピックに目標角度をpublishすれば、パンのサーボモータを動かすことができます。

joint_state_publisherのGUIをコントローラとして利用する

毎回、コマンドを入力して目標角度をpublishするのも面倒です。以前、URDFを書いたモデルをRViz上で動かしたときには、Joint state publisherのGUIをにより、スライドバーで簡単に関節を動かすことができました。このように関節の目標角度を設定し、実機を動作させたいものです。

そこで、Joint state publisherからのメッセージをpan, tiltのcommandに送るようにしてみます。launchファイルを次のようにします。


<launch>
   <arg name="model" default="$(find ptcam_description)/urdf/pan_tilt_camera.urdf"/>
   <arg name="gui" default="True" />
   <param name="robot_description" textfile="$(arg model)" />
   <param name="use_gui" value="$(arg gui)"/>
   
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <remap from="joint_states" to="joint_commands"/>
   </node>

   <node name="cmd_transform_p" pkg="topic_tools" type="transform" args="/joint_commands /pan/command std_msgs/Float64 'm.position[0]'"/>
   <node name="cmd_transform_t" pkg="topic_tools" type="transform" args="/joint_commands /tilt/command std_msgs/Float64 'm.position[1]'"/>
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

   <node name="ptcam" pkg="ros_arduino_python" type="arduino_node.py" output="screen" clear_params="true">
      <rosparam file="$(find ptcam_arduino)/config/ptcam_arduino_params.yaml" command="load" />
   </node>
</launch>
		

中程で、joint_state_publisherノードを起動していますが、そのときにpublishするトピックの名前を"joint_states"から"joint_commands"に変更しています。

joint_commandsは、positionの組(パンとチルト)で送られるので、それを2つに分ける必要があります。これを、topic_toolsのtransformによって行います。 joint_commands/position[0]を/pan/commandに、joint_commands/position[1]を/tilt/commandに送ります。

その次の行では、robot_state_publisherノードを起動していますが、これについては後ほど説明します。

このlaunchファイルで起動すると、Joint state publisherによるスライドバーが現れ、これを使って実機を動かすことができます。

yamlファイルで速度をinit_speed: 20と指定したので、目標角度を大きく設定しても、ゆっくりとその角度まで移動していきます。

これは、RCサーボに最終目標角度をただ送りつけているのではなく、Arduino上のコントローラが、指定速度になるように角度指令を少しずつ変化させているためです。 しっかり制御が働いていることがわかります。

速度のパラメータを大きくすると、制御の効果が見えにくいので、あえて速度を落として動作させてみました。

RVizで実機の状態を表示

ptcamノード(arduino_node.py)は、現在の関節の状態を/joint_commandsトピックにsubscribeしています。このトピックには、角度と速度の情報などが含まれています。

$ rostopic echo /joint_states
header: 
  seq: 13538
  stamp: 
    secs: 1458821488
    nsecs: 832928895
  frame_id: ''
name: ['tilt', 'pan']
position: [0.0, 0.7330382858376185]
velocity: [0.0, 0.5235987755983018]
effort: []
		

この情報をもとに、RVizで現在のロボットの状態を表示することができます。

前のlaunchファイルで、robot_state_publisherノードを起動していたのは、これを行うためです。robot_state_publisherは、joint_statesメッセージをsubscribeし、リンクのジオメトリ(TF)に変換し、これを/tfトピックとしてsubscribeします。RVizは、/tfトピックとURDFをもとに、画面上にロボットを表示するという仕組みです。

またlaunchファイルでは、"robot_description"パラメータにパンチルトカメラのURDFを指定しています。そのためrosrun rviz rvizとしてRVizを起動し、RVizで"RobotModel"を表示すれば、実機パンチルトカメラの現在の状態がグラフィカルに表示されます。

Joint state publisherのスライドバーを動かすと、実機カメラが動き、それに合わせてRViz上の表示も同じように動くことが確認できます。

試しに関節の速度パラメータを変更し、サーボを速く動かしてみるとよくわかるのですが、実機とRVizの表示にズレが出ています。これは/joint_statesのpositionが本当の値からずれているために起こります。前に説明したとおり、コントローラはRCサーボの角度センサから情報を得ているわけではなく、現在の角度を精度よく推定できていないためです。

この問題は、角度センサの情報を得たり、サーボの遅れを考慮した推定をするなど、現在状態の推定の精度を上げることで解決できます。