gazebo_ros_pkgのgazebo_ros_bumperでstateが更新されない時に確認すること

はじめに

gazebo_ros_pkgsのgazebo_ros_bumperを使用した時に、rostopic echoで表示するとstateに値が反映されなかったため、調査をしました。

結論からいうとgazebo-contact-sensor-added-to-a-robot-link-in-urdf-file-is-not-working-as-expectedの回答どおりの結果だったのですが、目を疑う内容だったため下記にチェックする方法をまとめます。

前提条件

環境	バージョン
開発機	Ubuntu 16.04.6 LTS
ROS	Kinetic Kame
Gazebo	version 9.11.0
プラグイン	gazebo9-ros-pkgs

XACROの設定

ある車体の前後にバンパーを設置するXACRO例を下記に示します。

<xacro:macro name="bumper" params="front_rear reflect_front_rear">
    <link name="bumper_${front_rear}">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="${width} ${bumper_height} ${bumper_depth}"/>
            </geometry>
            <material name="dark"/>
        </visual>
        <collision name="bumper_${front_rear}_collision">
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry name="bumper_${front_rear}_collision">
                <box size="${width - collision_avoidance} ${bumper_height - collision_avoidance} ${bumper_depth - collision_avoidance}"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="${mass_bamper}"/>
            <inertia ixx="0.003021" ixy="0.0" ixz="0" iyy="0.021871" iyz="0.0" izz="0.019683"/>
        </inertial>
    </link>
    <joint name="${front_rear}_bumper" type="fixed">
        <parent link="body_link"/>
        <child link="bumper_${front_rear}"/>
        <origin rpy="0 0 0" xyz="0 ${reflect_front_rear * (height/2 + bumper_height/2)} ${bumper_z - depth/2}"/>
    </joint>

    <gazebo reference="bumper_${front_rear}">
        <material>Gazebo/Red</material>
        <selfCollide>true</selfCollide>
        <sensor name="bumper_${front_rear}_sensor" type="contact">
            <contact>
                <collision>bumper_${front_rear}_collision</collision>
            </contact>
            <plugin name="gazebo_ros_bumper_controller" filename="libgazebo_ros_bumper.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>10.0</updateRate>
                <bumperTopicName>bumper_${front_rear}</bumperTopicName>
                <frameName>world</frameName>
            </plugin>
        </sensor>
    </gazebo>
</xacro:macro>

URDFからSDFを生成する

ここでURDFからGazeboに読み込まれるときにどのように変換されるのかSDFを手動で生成して確認します。生成するときは、下記のようなシェルで実行できます。

# !/usr/bin/env bash

CURRENT_DIR=$(pwd)

# スクリプトが存在するディレクトリ
SCRIPT_DIR=$(cd $(dirname $0);pwd)
PRJ_HOME=${SCRIPT_DIR}/..

echo "=== COMPILE ==="
cd ${PRJ_HOME}/../../

colcon build || exit
source install/local_setup.bash

echo "=== CLEAN UP ==="
cd ${PRJ_HOME}
rm my_package/urdf/gazebo/setup.xacro.urdf my_package/urdf/gazebo/setup.xacro.urdf.sdf

echo "=== URDF CREATED START ==="
rosrun xacro xacro my_package/urdf/gazebo/setup.xacro --inorder > my_package/urdf/gazebo/setup.xacro.urdf

echo "=== URDF PARSE CHECK START ==="
check_urdf my_package/urdf/gazebo/setup.xacro.urdf

echo "=== SDF PARSE START ==="
gz sdf -p my_package/urdf/gazebo/setup.xacro.urdf > my_package/urdf/gazebo/setup.xacro.urdf.sdf

※この例では、XACROがパッケージのファイルとして扱われており、参照しているため、ビルドを実行しています。

シェルを実行するとURDFとSDFが生成できます。gazebo_ros_bumperで使用する部分を抜粋して下記に示します。

URDFの場合は、次のようになります。

<gazebo reference="bumper_front">
    <material>Gazebo/Red</material>
    <selfCollide>true</selfCollide>
    <sensor name="bumper_front_sensor" type="contact">
        <contact>
            <collision>bumper_front_collision_collision</collision>
        </contact>
        <plugin filename="libgazebo_ros_bumper.so" name="gazebo_ros_bumper_controller">
            <alwaysOn>true</alwaysOn>
            <updateRate>10.0</updateRate>
            <bumperTopicName>bumper_front</bumperTopicName>
            <frameName>world</frameName>
        </plugin>
    </sensor>
</gazebo>

SDFの場合は、次のようになります。

<collision name='base_link_fixed_joint_lump__bumper_front_collision_collision_2'>
    <pose frame=''>0 -0.95 0.22 0 -0 0</pose>
    <geometry>
        <box>
            <size>0.679 0.099 0.249</size>
        </box>
    </geometry>
    <surface>
        <contact>
            <ode/>
        </contact>
        <friction>
            <ode/>
        </friction>
    </surface>
</collision>

ここでXACROで設定しているcollision_nameは、bumper_${front_rear}_collisionと設定しているのに、URDFとして出力されるときは、bumper_front_collision_collisionとsuffixに_collisionが自動的に追加され、それをSDFに変換するとbase_link_fixed_joint_lump__bumper_front_collision_collision_2というprefixにbase_link_fixed_joint_lump__とsuffixに_2が追加されています。

XACROで設定した名称とSDFに最終的に出力されている名称とは異なるため、Gazeboのsensor情報にcontactを手動で設定する必要があります。

手動でcontact_collisionを設定する

前述でどのcollision nameを設定すればよいかわかったので、XACROを修正します。

<xacro:macro name="bumper" params="front_rear reflect_front_rear collision_name">
    <link name="bumper_${front_rear}">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="${width} ${bumper_height} ${bumper_depth}"/>
            </geometry>
            <material name="dark"/>
        </visual>
        <collision name="bumper_${front_rear}_collision">
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry name="bumper_${front_rear}_collision">
                <box size="${width - collision_avoidance} ${bumper_height - collision_avoidance} ${bumper_depth - collision_avoidance}"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="${mass_bamper}"/>
            <inertia ixx="0.003021" ixy="0.0" ixz="0" iyy="0.021871" iyz="0.0" izz="0.019683"/>
        </inertial>
    </link>
    <joint name="${front_rear}_bumper" type="fixed">
        <parent link="body_link"/>
        <child link="bumper_${front_rear}"/>
        <origin rpy="0 0 0" xyz="0 ${reflect_front_rear * (height/2 + bumper_height/2)} ${bumper_z - depth/2}"/>
    </joint>

    <gazebo reference="bumper_${front_rear}">
        <material>Gazebo/Red</material>
        <selfCollide>true</selfCollide>
        <sensor name="bumper_${front_rear}_sensor" type="contact">
            <contact>
                <collision>${collision_name}</collision>
            </contact>
            <plugin name="gazebo_ros_bumper_controller" filename="libgazebo_ros_bumper.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>10.0</updateRate>
                <bumperTopicName>bumper_${front_rear}</bumperTopicName>
                <frameName>world</frameName>
            </plugin>
        </sensor>
    </gazebo>
</xacro:macro>

と変更し、下記のように生成します。

<xacro:bumper front_rear="front" reflect_front_rear="-1" collision_name="base_link_fixed_joint_lump__bumper_front_collision_collision_2" />

これで再度実行すると、gazebo_ros_bumperのstateが正常に取得することができます。ただし、自動で生成されるcollision nameは、URDFを組み替えたりすることによって変わってしまうため、変更のたびに修正する必要があります。

この問題は、2019年12月31日現在まだ解決していません。

まとめ

• XACROで設定したcollision nameは、URDFに変換するとsuffixに_collisionがつき、SDFに変換するとプロジェクトに沿ったprefixとsaffixがつく
• contactのcollisionは、手動でURDF及びSDFを生成後、変換された値を元に設定する必要がある
• 自動で生成されるcollision nameは、URDFを組み替えたりすることによって変わってしまうため注意する必要がある