URDF: camera origins broken if link rs200_camera is not direct child of world

[Maik93]

Hi, I'm trying to figure out how to attach your rs200 to my robot's URDF: the problem is that while using your URDF structure (rs200_simulation.xacro) everything is ok, each camera has its vertex in the correct position: (it can be seen by setting <visualize>1</visualize> in realsense-RS200.macro.xacro:151,173,195)

However if I attach the camera to any other link that isn't called world each camera cone collapse in rs200 origin, as follows:

This result can be reproduced using this simple URDF:

<?xml version="1.0"?>
<!--Develped by Daniel Ordonez 22.05.2018 - daniels.ordonez@gmail.com
  INFORMATION:
    This is an example of how to use the realsense-rs200 macro function.
-->
<robot name="robot_with_rs200" xmlns:xacro="http://ros.org/wiki/xacro">
    <!-- Import macro for realsense-RS200 camera-->
    <xacro:include filename="$(find realsense_gazebo_plugin)/urdf/realsense-RS200.macro.xacro"/>
    <!-- Create camera instance -->
    <xacro:realsense-rs200/>

    <!-- Gazebo world link required to position the robot with respect to origin-->
    <link name="world"/>

    <link name="box">
        <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
            <box size="0.1 0.1 0.1"/>
        </geometry>
        </visual>
    </link>

    <!-- Place camera referenced to box frame-->
    <joint name="realsense_joint" type="fixed">
        <parent link="box"/>
        <child link="rs200_camera"/>
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
    </joint>

    <joint name="world_joint" type="fixed">
        <parent link="world"/>
        <child link="box"/>
        <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </joint>
</robot>

[Danfoa]

Hey @Maik93, as far as I notice you have the error of renaming the connection joint between the camera and its parent link (this could be the cause of the issue, but I am not sure), so if you notice in the realsense-RS200.macro.xacro file, in the lines 41 to 46:

    <!-- Connect camera to parent frame-->
    <joint name="${prefix}realsense_joint" type="fixed">
        <parent link="${parent}"/>
        <child link="${prefix}rs200_camera"/>
        <xacro:insert_block name="origin"/>
    </joint>

The macro function is coded in such a way that when you create an instance of the camera object you provide the sufficient parameters to create this joint, so If i take your file and turn it this way:

<?xml version="1.0"?>
<!--Develped by Daniel Ordonez 22.05.2018 - daniels.ordonez@gmail.com
  INFORMATION:
    This is an example of how to use the realsense-rs200 macro function.
-->
<robot name="robot_with_rs200" xmlns:xacro="http://ros.org/wiki/xacro">
    <!-- Import macro for realsense-RS200 camera-->
    <xacro:include filename="$(find realsense_gazebo_plugin)/urdf/realsense-RS200.macro.xacro"/>

    <!-- Create camera instance -->
    <xacro:realsense-rs200 prefix="" parent="box">  <!-- Do not forget parameters-->
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
    </xacro:realsense-rs200>

    <!-- Gazebo world link required to position the robot with respect to origin-->
    <link name="world"/>

    <link name="box">
        <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
            <box size="0.1 0.1 0.1"/>
        </geometry>
        </visual>
    </link>

    <joint name="world_joint" type="fixed">
        <parent link="world"/>
        <child link="box"/>
        <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </joint>
</robot>

Then the urdf function is called appropriately, and as far as I notice the tree link is created properly and no axis collide, as you can see here:

(I use Rviz because is faster and both Rviz and Gazebo check the same robot_state topic.

PS: I will update the instructions of use of the macro function, I notice they could be a little deceiving, and please let me know if the problem was solved.

[Maik93]

Hi @Danfoa, thanks for your suggestion, you make me notice that kinetic-devel and master differ even in this files. This morning, before your comment, I've worked out a solution that works for Kinetic in pull request #27. I can't test yours because I'm not running ROS Melodic and I can't switch to master.

The only think I can say is that even for me Rviz was ok, the problem was in the part of the URDF regarding Gazebo plugins, thus only in Gazebo you can see cameras with wrong origins (even if their TF were correct on Rviz). Please look if it happen to you too, if so we can work together to merge my pull request on master too.