moveit_msgs/DisplayRobotStateを利用してrviz上にロボットを表示させる

rviz上に自由にロボットを配置し、可視化するにはmoveit_msgs/DisplayRobotStateが便利です。 これを利用することで、関節角度+ルートリンク位置を指定することでロボットを可視化できます。

サンプルはroslaunch jsk_interactive_marker sample_display_robot_state.launchで確認できます。

moveit_msgs/DisplayRobotStateを利用するためには、moveit_ros_visualizationからRobotStateを選択します.

このRobotStateプラグインは2つのパラメータ、1つのトピックに依存します。

• parameter

  • /robot_description

    ロボットの形状を読むために、robot_descriptionを必要とします。

  • /robot_description_semantic

    moveitで利用される、マニピュレータ情報等を保持するパラメータ。一般にはsrdfというファイルで管理されることが多い。 仮想ジョイントを追加することができるため、これを利用してルートリンクに6自由度の仮想ジョイントを追加してrviz上にロボットを配置させる。

• topic

  • Robot State Topic (moveit_msgs/DisplayRobotState)

    トピック名はプラグインのフォームで設定可能。

ロボットのルートリンクを指定するため、以下のようなsrdfを/robot_description_semanticsに設定する必要があります。

<?xml version="1.0" ?>
<robot name="JAXON_RED">
    <virtual_joint name="world_joint" type="floating" parent_frame="odom" child_link="BODY" />
    <passive_joint name="world_joint" />
</robot>

このsrdfはmoveitでのプランニングを行うためには情報が少なすぎますが、可視化には十分です。 このようなsrdfを自動的に設定するためのスクリプトが package://jsk_interactive_marker/scripts/semantic_robot_state_generator.py として提供されています。

このスクリプトで設定されるsrdfはworld_jointという仮想ジョイントがルートリンクとオドメトリ原点との間に設定されます。 これはmoveit標準の命名則と一致しているため、moveit_configで生成したsrdfをsemantic_robot_state_generator.pyの代わりに用いることもできます。

euslispからmoveit_msgs/DisplayRobotStateを生成する関数としてangle-vector-to-display-robot-stateがpackage://jsk_interactive_marker/euslisp/display-robot-state.lにて提供されています。 これを用いると、以下のようにしてrviz上にロボットを可視化できます。

(load "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-interface.l")
(setq *robot* (instance jaxon_red-robot :init))
(send *robot* :fix-leg-to-coords (make-coords))
(ros::roseus "foo")
(ros::advertise "/robot_state" moveit_msgs::DisplayRobotState)
(ros::publish "/robot_state" (angle-vector-to-display-robot-state *robot* (send (send *robot* :link "BODY") :copy-worldcoords)))

注意点

rvizでRobotStateを追加する前にrobot_description_stateが設定されている必要があります。 そうでないと、ルートリンクの表示がおかしくなります。