support multi visual tags

[eisoku9618]

cc @YoheiKakiuchi (original comitter of this part https://github.com/ros/robot_model/pull/20)

I am a urdf_to_collada user which converts URDF to COLLADA, and it seems that the original URDF file and the generated COLLADA are different when it has multi visual tags.

Let's say we have the following URDF file test.urdf.

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="test" >
  <link name="base_link" />

  <joint name="test_joint_1" type="fixed">
    <parent link="base_link"/>
    <child link = "link1"/>
  </joint>

  <link name="link1">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.5 0.3"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1 1 1"/>
      </material>
    </visual>
    <visual>
      <origin xyz="0 0 0.2" rpy="1.57 0 0"/>
      <geometry>
        <box size="0.1 1.0 0.1"/>
      </geometry>
      <material name="White">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
  </link>
</robot>

original URDF	COLLADA w/o this PR	COLLADA w/ this PR
roslaunch urdf_tutorial display.launch model:=test.urdf	rosrun collada_urdf urdf_to_collada test.urdf test.dae && roslaunch urdf_tutorial display.launch model:=test.dae	rosrun collada_urdf urdf_to_collada test.urdf test.dae && roslaunch urdf_tutorial display.launch model:=test.dae

[eisoku9618]

I'm not familiar with these modelling programs, and also the original code seems to separate intentionally the first element from the others of visual tags, so I'm not sure that this PR is correct. Could you take a look when you have a time cc @YoheiKakiuchi -san

[YoheiKakiuchi]

LGTM

original code is quite old. At that time, definition of 'urdf' was different from current version. And, I think multiple visual/collision was not tested enough.

[eisoku9618]

I see. Thank you.

@clalancette could you review?

[clalancette]

@eisoku9618 I'm sorry for the delay here.

In general, the code looks OK to me. There's a few things I will mention about this, though:

1. It might be nice to change _WriteGeometry so that it doesn't have a default of NULL, but instead it always requires org_trans to be passed in. Since there are only a couple of call sites left after this PR, I think that makes a little more sense.
2. We would want to add a test to go along with this PR to make sure that this problem is fixed.
3. To be frank, we do not have extensive tests for this package for other collada files. The worry is that we could be breaking things without knowing it. There are a couple of different ways we could mitigate that. One is to add a lot more testing to this package, which is a long effort. The other is to put this on a "noetic" branch, so it would be targeted for the next ROS release. That would ensure that we don't break existing code.

@sloretz , any additional thoughts here?

[eisoku9618]

@clalancette Thank you for your review.

We would want to add a test to go along with this PR to make sure that this problem is fixed.

Could you give me any suggestion for the way to test this PR?

TEST(urdf_to_collada, multi_visual_tags)
{
  urdf::Model robot_model_urdf, robot_model_collada;
  // load original urdf file
  robot_model_urdf.initFile("test.urdf");
  // convert urdf to collada
  collada_urdf::WriteUrdfModelToColladaFile(robot_model_urdf, "test.dae");
  // load converted collada file
  robot_model_collada.initFile("test.dae");
  // check whether robot_model_urdf and robot_model_collada are the same
  ASSERT_TRUE(check_identity(robot_model_urdf, robot_model_collada));
}

I tried to write a test like the above, but I could not implement check_identity part because WriteUrdfModelToColladaFile, which converts a URDF file to a COLLADA file, put the multi visual geometries to one mesh geometry. For example, in https://github.com/ros/collada_urdf/pull/29#issue-237131619 example, Cyan geometry and White geometry are merged into one mesh geometry.

• https://github.com/ros/collada_urdf/blob/kinetic-devel/collada_parser/src/collada_parser.cpp#L1193

So, it seems difficult for me to check White geometry is properly translated.