Xacro:include unexpected behavior

Hi @Blazej_Sowa,

On desktop:
I’ve developed a xacro package called leo_ssom_addons_description. I’ve tested it and it’s functioning as expected.

On the rover:
When I’m including it in the robot.urdf.xacro file and restarting the leo service, no error is thrown but the addons aren’t displayed in the RobotModel and the TF aren’t displayed either in RViz.

<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
       name="leo">

  <xacro:include filename="$(find leo_description)/urdf/macros.xacro"/>

  <xacro:leo default_antenna="true"
             rockers_fixed="true"
             footprint_link="true"
             link_prefix=""
             joint_prefix=""/>

  <xacro:include filename="$(find leo_ssom_addons_description)/urdf/addons.xacro"/>
</robot>

The package is properly installed and listed by rospack without running source ws/devel/setup.bash.

rospack list | grep leo_
leo_bringup /opt/ros/noetic/share/leo_bringup
leo_description /opt/ros/noetic/share/leo_description
leo_fw /opt/ros/noetic/share/leo_fw
leo_ssom_addons_description /home/pi/Code/leo_ws/src/leo_ssom_addons_description
leo_teleop /opt/ros/noetic/share/leo_teleop
leo_viz /opt/ros/noetic/share/leo_viz

I’m testing on LeoOS 1.0.0beta1 (Ubuntu 20.04.3 LTS 5.4.0-1047-raspi) at the moment.

I followed the Leo Rover Integrations - PhantomX Pincher tutorial as inspiration for what I need to do.

content of addons.xacro:

<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/e_stop.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/mounting_plates.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/realsense.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/jetson_nano.xacro" />
</robot>

content of e_stop.xacro

<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/ssom_common.xacro" />
    <xacro:ssom_link link_name="estop_guard_link" link_mesh="EStopGuard" link_origin_xyz="0 0 0" link_origin_rpy="0 ${pi/2} -${pi/2}" has_collision_mesh="true"/>
    <xacro:ssom_joint joint_name="estop_guard_joint" type="fixed" parent="base_link" child="estop_guard_link" joint_origin_xyz="-0.178 0 -0.02" joint_origin_rpy="0 0 0" joint_axis_xyz="0 0 0" />
</robot>

content of ssom_common.xacro:

<?xml version="1.0" encoding="utf-8"?>
<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Adapted from the kinova-ros pkg -->

  <xacro:macro name="ssom_link" params="link_name link_mesh link_origin_xyz link_origin_rpy has_collision_mesh:=false">
    <link name="${link_name}">
      <visual>
        <geometry>
          <mesh filename="package://leo_ssom_addons_description/meshes/visual/${link_mesh}.dae"/>
        </geometry>
        <origin xyz="${link_origin_xyz}" rpy="${link_origin_rpy}"/>
      </visual>
      <collision>
        <geometry>
          <xacro:if value="${has_collision_mesh}">
            <mesh filename="package://leo_ssom_addons_description/meshes/collision/${link_mesh}.stl"/>
          </xacro:if>
          <xacro:unless value="${has_collision_mesh}">
            <mesh filename="package://leo_ssom_addons_description/meshes/visual/${link_mesh}.dae"/>
          </xacro:unless>
        </geometry>
        <origin xyz="${link_origin_xyz}" rpy="${link_origin_rpy}"/>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="ssom_joint" params="joint_name type parent child joint_origin_xyz joint_origin_rpy joint_axis_xyz">
    <joint name="${joint_name}" type="${type}">
      <parent link="${parent}"/>
      <child link="${child}"/>
      <axis xyz="${joint_axis_xyz}"/>
      <origin xyz="${joint_origin_xyz}" rpy="${joint_origin_rpy}"/>
    </joint>
  </xacro:macro>

  <xacro:macro name="virtual_link" params="link_name">
    <link name="${link_name}"/>
  </xacro:macro>

  <xacro:macro name="virtual_joint" params="joint_name type parent child joint_axis_xyz joint_origin_xyz joint_origin_rpy joint_lower_limit joint_upper_limit">
    <joint name="${joint_name}" type="${type}">
      <parent link="${parent}"/>
      <child link="${child}"/>
      <axis xyz="${joint_axis_xyz}"/>
      <limit effort="2000" velocity="1" lower="${joint_lower_limit}" upper="${joint_upper_limit}"/>
      <origin xyz="${joint_origin_xyz}" rpy="${joint_origin_rpy}"/>
    </joint>
  </xacro:macro>
</root>

Can you try manually running:

xacro /etc/ros/urdf/robot.urdf.xacro

and check if the outputted URDF does not have any obvious errors?

Here’s the output of xacro /etc/ros/urdf/robot.urdf.xacro:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /etc/ros/urdf/robot.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="leo">
  <!-- LEO GAZEBO -->
  <link name="base_footprint"/>
  <link name="base_link">
    <inertial>
      <mass value="1.584994"/>
      <origin xyz="-0.019662 0.011643 -0.031802"/>
      <inertia ixx="0.01042" ixy="0.001177" ixz="-0.0008871" iyy="0.01045" iyz="0.0002226" izz="0.01817"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Chassis.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Chassis_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_L_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_R_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelA.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelA.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelB.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelB.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="antenna_link">
    <inertial>
      <mass value="0.001237"/>
      <origin xyz="0 0 0.028828"/>
      <inertia ixx="2.5529e-7" ixy="0.0" ixz="0.0" iyy="2.5529e-7" iyz="0.0" izz="1.354e-8"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Antenna.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.028"/>
      <geometry>
        <cylinder length="0.056" radius="0.0055"/>
      </geometry>
    </collision>
  </link>
  <link name="camera_frame"/>
  <link name="camera_optical_frame"/>
  <!-- JOINTS -->
  <joint name="base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.19783"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <joint name="rocker_L_joint" type="fixed">
    <origin rpy="0 0 3.141592653589793" xyz="0.00263 0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_L_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
  </joint>
  <joint name="rocker_R_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.00263 -0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_R_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
    <mimic joint="rocker_L_joint"/>
  </joint>
  <joint name="wheel_FL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_FL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_RL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_FR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_FR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_RR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="antenna_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.0052 0.056 -0.0065"/>
    <parent link="base_link"/>
    <child link="antenna_link"/>
  </joint>
  <joint name="camera_joint" type="fixed">
    <origin rpy="0 0.2094 0" xyz="0.0971 0 -0.0427"/>
    <parent link="base_link"/>
    <child link="camera_frame"/>
  </joint>
  <joint name="camera_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0.0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_frame"/>
    <child link="camera_optical_frame"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="estop_guard_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/EStopGuard.dae"/>
      </geometry>
      <origin rpy="0 1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/EStopGuard.stl"/>
      </geometry>
      <origin rpy="0 1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="estop_guard_joint" type="fixed">
    <parent link="base_link"/>
    <child link="estop_guard_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.178 0 -0.02"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="front_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/FrontMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/FrontMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="front_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0.11 0 0.0085"/>
  </joint>
  <link name="rear_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/RearMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.155 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/RearMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.155 0 -0.0085"/>
    </collision>
  </link>
  <joint name="rear_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="rear_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.045 0 0.0085"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="d435i_holder_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/D435iHolder.dae"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/D435iHolder.stl"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="d435i_holder_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="d435i_holder_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.050 -0.018 0"/>
  </joint>
  <material name="aluminum">
    <color rgba="0.5 0.5 0.5 1"/>
  </material>
  <material name="plastic">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="realsense_joint" type="fixed">
    <origin rpy="0 0.087266 0" xyz="0.024 -0.0145 0.0075"/>
    <parent link="d435i_holder_base_link"/>
    <child link="realsense_bottom_screw_frame"/>
  </joint>
  <link name="realsense_bottom_screw_frame"/>
  <joint name="realsense_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.010600000000000002 0.0175 0.0125"/>
    <parent link="realsense_bottom_screw_frame"/>
    <child link="realsense_link"/>
  </joint>
  <link name="realsense_link">
    <visual>
      <!-- the mesh origin is at front plate in between the two infrared camera axes -->
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0043 -0.0175 0"/>
      <geometry>
        <mesh filename="package://realsense2_description/meshes/d435.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0175 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
    </collision>
    <inertial>
      <!-- The following are not reliable values, and should not be used for modeling -->
      <mass value="0.072"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
    </inertial>
  </link>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="jetson_nano_bracket_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/JetsonNanoBracket.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.079 0.0225 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/JetsonNanoBracket.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.079 0.0225 -0.0085"/>
    </collision>
  </link>
  <joint name="jetson_nano_bracket_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="jetson_nano_bracket_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.079 -0.0225 0.0085"/>
  </joint>
  <link name="jetson_nano_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/JetsonNanoSimplified.dae"/>
      </geometry>
      <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/JetsonNanoSimplified.stl"/>
      </geometry>
      <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="jetson_nano_base_joint" type="fixed">
    <parent link="jetson_nano_bracket_link"/>
    <child link="jetson_nano_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="left_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/WifiAntenna.dae"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/WifiAntenna.stl"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="left_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="left_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.090 0.054 0"/>
  </joint>
  <link name="right_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/WifiAntenna.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/WifiAntenna.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="right_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="right_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.090 -0.054 0"/>
  </joint>
</robot>

I don’t see an obvious error here, plus the same macro was working as expected on my desktop.

I retried it this morning and for some reason, it’s now working! The TF are shown in the tree.