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

# はじめに

[gazebo_ros_pkgs](http://wiki.ros.org/gazebo_ros_pkgs)の[gazebo_ros_bumper](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_bumper.cpp)を使用した時に、`rostopic echo`で表示するとstateに値が反映されなかったため、調査をしました。

結論からいうと[gazebo-contact-sensor-added-to-a-robot-link-in-urdf-file-is-not-working-as-expected](http://answers.gazebosim.org/question/15095/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を組み替えたりすることによって変わってしまうため、変更のたびに修正する必要があります。


[この問題](https://bitbucket.org/osrf/sdformat/issues/113/parsing-from-urdf-to-sdf-does-not-handle)は、2019年12月31日現在まだ解決していません。

# まとめ

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