multisense_description naming inconsistencies

Issue #61 resolved
Marco Camurri
created an issue

Hi all, I'm into integrating the Multisense SL URDF into our robot.

To do so I was taking inspiration from the robot xacro file here: https://bitbucket.org/crl/multisense_ros/src/981a964f5131a467e4694b365acbacf2947e8acc/multisense_description/robots/multisense_sl.urdf.xacro?at=default&fileviewer=file-view-default

but this file is trying to find a non-existing file multisense_sl.urdf (although multisenseSL.urdf exists) in the non-existing package multisense_sl_description (although multisense_description exists)

Is this because of renaming issues?

What is the correct way to integrate the multisense sl on a robot?

Usually we have a xacro file of the sensors, so we can add at least the origin as a parameter, but in this case I just found the urdf alone (multisenseSL.urdf).

Comments (4)

  1. Marco Camurri reporter

    Quick update: we were able to integrate the multisense with our URDF, but we had to copy paste the content of multisenseSL.urdf into a xacro file and prepend the tf_prefix to every dynamic joint in order to work it out.

    The xacro looks like the following

    <?xml version="1.0" ?>
    <robot xmlns:xacro="http://ros.org/wiki/xacro">
    
    <xacro:macro name="multisense_sl" params="parent *origin prefix" >
    
      <joint name="${parent}_head_root_joint" type="fixed">
        <insert_block name="origin"/>
        <parent link="${parent}"/>
        <child link="head_root"/>
      </joint>
      <!-- Head Link Simplified Collision Mesh -->
      <link name="head_root"/>
      <joint name="head_root_joint" type="fixed">
        <parent link="head_root"/>
        <child link="head"/>
      </joint>
      <link name="head">
        <inertial>
          <origin xyz="-0.075493 3.3383E-05 0.02774" rpy="0 0 0" />
          <mass value="1.4199" />
          <inertia iyy="0.0041178" ixy="-1.5797E-06" iyz="-6.8415E-07" ixx="0.0039688" ixz="-0.00089293" izz="0.0035243" />
        </inertial>
        <visual>
          <origin xyz="0 0  0" rpy="0 0 0" />
          <geometry>
              <mesh filename="package://multisense_description/meshes/head.dae" />
          </geometry>
          <material name="">
            <color rgba="0.9098 0.44314 0.031373 1" />
          </material>
        </visual>
      <collision>
        <origin xyz="-0.0503 0 -0.00195" rpy="0 0 0" />
        <geometry>
          <box size="0.1311 0.12 0.0591"/>
        </geometry>
      </collision>
      </link>
    
    
      <joint name="left_camera_optical_joint" type="continuous">
        <parent link="${prefix}/left_camera_optical_frame" />
        <child link="${prefix}/motor" />
      </joint>
    
      <link name="${prefix}/motor"/>
      <link name="${prefix}/spindle"/>
    
      <joint name="motor_joint" type="continuous">
        <parent link="${prefix}/motor" />
        <child link="${prefix}/spindle" />
        <axis xyz="0 0 1" />
      </joint>
    
      <joint name="spindle_joint" type="continuous">
        <parent link="${prefix}/spindle" />
        <child link="${prefix}/hokuyo_link" />
      </joint>
    
      <link name="${prefix}/hokuyo_link">
        <inertial>
          <origin xyz="0.012428 0.0004084 -0.0041835" rpy="0 0 0" />
          <mass value="0.057664" />
          <inertia iyy="4.2412E-05" ixy="4.9927E-08" iyz="-9.8165E-09" ixx="3.7174E-05" ixz="1.1015E-05" izz="4.167E-05" />
        </inertial>
        <visual>
          <origin xyz="-.03 -.1 0" rpy="-1.9179632679 -1.57079632679 0" />
          <geometry>
            <mesh filename="package://multisense_description/meshes/head_camera.dae" />
          </geometry>
          <material name="">
            <color rgba="0.72941 0.35686 0.023529 1" />
          </material>
        </visual>
      <collision>
        <origin xyz="0.03 0 0.0235" rpy="0 0 0" />
        <geometry>
          <cylinder radius="0.024425" length="0.047"/>
        </geometry>
      </collision>
      </link>
    
      <link name="${prefix}/head_hokuyo_frame"/>
      <joint name="head_hokuyo_frame_joint" type="fixed"> 
        <!--
         This transform accounts for the fact ROS expects laser data in the x forward,
         y left, z up frame and the calibraton has the data in x left, y up, z forward.
         Thus a roll of -90 * a pitch of -90 will make the appropriate transform. Note
         Roll and pitch are both with respect to the origional x left, y up, z forward frame
         -->
        <origin xyz="0 0 0" rpy="-1.57079632679 -1.57079632679 0.0" />
        <parent link="${prefix}/hokuyo_link" />
        <child link="${prefix}/head_hokuyo_frame"/>
      </joint>
    
      <!-- Stereo Camera -->
      <joint name="left_camera_frame_joint" type="fixed">
        <!-- optical frame collocated with tilting DOF -->
        <origin xyz="0.0 0.035 -0.002"/>
        <parent link="head"/>
        <child link="left_camera_frame"/>
      </joint>
      <link name="left_camera_frame">
        <inertial>
          <mass value="0.1" />
          <origin xyz="0 0 0" />
          <inertia ixx="0.001"  ixy="0.0"  ixz="0.0" iyy="0.001"  iyz="0.0" izz="0.001" />
        </inertial>
      </link>
      <joint name="left_camera_optical_frame_joint" type="fixed">
        <origin xyz="0 0 0" rpy="-1.57079632679 0.0 -1.57079632679" />
        <parent link="left_camera_frame" />
        <child link="${prefix}/left_camera_optical_frame"/>
      </joint>
      <link name="${prefix}/left_camera_optical_frame"/>
    
    
      <joint name="right_camera_frame_joint" type="fixed">
        <origin xyz="0.0 -0.035 -0.002"/>
        <parent link="head"/>
        <child link="right_camera_frame"/>
      </joint>
      <link name="right_camera_frame">
        <inertial>
          <mass value="0.1" />
          <origin xyz="0 0 0" />
          <inertia ixx="0.001"  ixy="0.0"  ixz="0.0" iyy="0.001"  iyz="0.0" izz="0.001" />
        </inertial>
      </link>
      <joint name="right_camera_optical_frame_joint" type="fixed">
        <origin xyz="0 0 0" rpy="-1.5708 0.0 -1.5708" />
        <parent link="right_camera_frame" />
        <child link="right_camera_optical_frame"/>
      </joint>
      <link name="right_camera_optical_frame"/>
    
      <joint name="center_top_led_frame_joint" type="fixed">
        <origin xyz="0.01125 0.0 0.0105"/>
        <parent link="head"/>
        <child link="center_top_led_frame"/>
      </joint>
      <link name="center_top_led_frame">
        <inertial>
          <mass value="0.1"/>
          <origin xyz="0 0 0"/>
          <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
        </inertial>
      </link>
      <joint name="center_bottom_led_frame_joint" type="fixed">
        <origin xyz="0.01125 0.0 -0.0155"/>
        <parent link="head"/>
        <child link="center_bottom_led_frame"/>
      </joint>
      <link name="center_bottom_led_frame">
        <inertial>
          <mass value="0.1"/>
          <origin xyz="0 0 0"/>
          <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
        </inertial>
      </link>
      <joint name="left_led_frame_joint" type="fixed">
        <origin xyz="-0.01443 0.07452 0.050346" rpy="0 -0.15 0.53"/>
        <parent link="head"/>
        <child link="left_led_frame"/>
      </joint>
      <link name="left_led_frame">
        <inertial>
          <mass value="0.1"/>
          <origin xyz="0 0 0"/>
          <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
        </inertial>
      </link>
      <joint name="right_led_frame_joint" type="fixed">
        <origin xyz="-0.01443 -0.07452 0.050346" rpy="0 -0.15 -0.53"/>
        <parent link="head"/>
        <child link="right_led_frame"/>
      </joint>
      <link name="right_led_frame">
        <inertial>
          <mass value="0.1"/>
          <origin xyz="0 0 0"/>
          <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
        </inertial>
      </link>
    
      <!-- IMU Frames, Mag/Accel and Gyro -->
      <!-- Include transfrom to the left_camera_frame (shift in -.035mm in X and -.002mm in Z) -->
    
      <link name="accel"/>
      <joint name="accel_joint" type="fixed">
        <origin xyz="-.0475 .0302 -.00275" rpy="0 1.5707 0"/>
        <parent link="head_root"/>
        <child link="accel"/>
      </joint>
    
      <link name="mag"/>
      <joint name="mag_joint" type="fixed">
        <origin xyz="-.0475 .0302 -.00275" rpy="0 1.5707 0"/>
        <parent link="head_root"/>
        <child link="mag"/>
      </joint>
    
      <link name="gyro"/>
      <joint name="gyro_joint" type="fixed">
        <origin xyz="-.0476 .0388 -.00075" rpy="1.5707 3.14159 1.5707 "/>
        <parent link="head_root"/>
        <child link="gyro"/>
      </joint>
    </xacro:macro>
    </robot>
    

    And to include it in our robot we do something like:

            <xacro:include filename="$(find myrobot_description)/urdfs/sensors/multisense_sl.urdf.xacro" />
    
           <!-- robot-related stuff -->
            <xacro:multisense_sl parent="trunk" prefix="myrobot">
                    <origin xyz="0.10 0 0" rpy="0 0 0"/>
            </xacro:multisense_sl>
    

    It's a kind of dirty trick, because I didn't want to copy-paste and modify the internals of your description. Maybe there's a better way? What about upgrading your official description in xacro format?

  2. Log in to comment