RobotLabs
  • Welcome
  • ESP32
    • PlatformIO
      • 1.1 Arduino Basics
      • 1.2 Install PlatformIO
      • 2.1 GPIO - LED Blink
      • 2.2 GPIO - LED Multi
      • 2.3 GPIO - LED Button
      • 3.1 PWM - LED Fade
      • 3.2 ADC - Analog Input
      • 4.1 IIC - OLED
    • MicroPython
    • ESP32 Projects
  • ROS
    • ROS2 Jazzy
      • 1.1 Install ROS 2 Jazzy
      • 1.2 Install VS Code
      • 1.3 Save ROS code on GitHub
      • 2.1 Create a Mobile Robot with URDF and Visualize it
      • 2.3 Create Launch Files to Display the URDF in RViz
      • 2.4 Simulation with Gazebo
      • 2.5 Gazebo sensors
      • 2.6 Sensor fusion and SLAM
    • MoveIt2
      • Moveit 2 joint position control with keyboard
      • Moveit2 joint velocity control for Panda robot arm
    • Perception
      • PCL-ROS
  • FOC
    • SimpleFOC
      • Install simpleFOC Lib
  • Template
    • Markdown
    • Interactive blocks
    • OpenAPI
    • Integrations
Powered by GitBook
On this page
  • Prerequisites
  • Create a Package
  • Add Meshe Files
  • Configure CMakeLists.txt and package.xml
  • Build the Package
  • Create the URDF File
  • Robot Base
  • Robot Wheels
  • RGBD Camera
  • Robot LIDAR
  • Robot IMU
  • Full Robot
  • URDF Explanation
  • Build the Package
  • Visualize the URDF in RViz
  1. ROS
  2. ROS2 Jazzy

2.1 Create a Mobile Robot with URDF and Visualize it

Previous1.3 Save ROS code on GitHubNext2.3 Create Launch Files to Display the URDF in RViz

Last updated 3 months ago

In this tutorial, we will create a mecanum mobile robot from scratch with URDF (Unified Robot Description Format), and then visualize the robot in RViz, a 3D visualization tool for ROS 2.

A Mecanum wheel is an omnidirectional wheel design to move in any direction, which is widely used in various robot products. It is far more fun and helpful to create a URDF file for a real-world robot you designed or you work with at your job or school.

Prerequisites

  • Installed urdf-tutorial package. sudo apt-get install ros-${ROS_DISTRO}-urdf-tutorial

Create a Package

The first step is to create a ROS 2 package in your ros workspace. Open a new terminal window, and create a new folder named mec_mobile.

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
mkdir mec_mobile
cd mec_mobile

1. Create the package where we will store our URDF file.

The mec_mobile_description package contains the robot description files that define the physical aspects of a robot, including its geometry, kinematics, dynamics, and visual aspects.

ros2 pkg create --build-type ament_cmake --license BSD-3-Clause mec_mobile_description

2. Then, let’s create a metapackage for this project.

ros2 pkg create --build-type ament_cmake --license BSD-3-Clause mec_mobile
cd mec_mobile

Configure your package.xml file in VS Code or gedit:

gedit package.xml
Make your package.xml file look like this:
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>mec_mobile</name>
  <version>0.0.0</version>
  <description>mecanum mobile robot by robotlabs(metapackage).</description>
  <maintainer email="robtlabs@todo.todo">ubuntu</maintainer>
  <license>BSD-3-Clause</license>
 
  <buildtool_depend>ament_cmake</buildtool_depend>   
  <exec_depend>mec_mobile_description</exec_depend> 
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
 
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

3. Add a README.md to describe what the package is about.

cd ..
gedit README.md
---------- Add following ----------
# mec_mobile #

I also recommend adding placeholder README.md files to the mec_mobile folder. The mec_mobile package is a metapackage. It contains lists of dependencies to other packages.

cd ~/ros2_ws/src/mec_mobile/mec_mobile
gedit README.md
---------- Add following ----------
# mec_mobile #

As well as the mec_mobile_description folder.

cd ~/ros2_ws/src/mec_mobile/mec_mobile_description
gedit README.md
---------- Add following ----------
# mec_mobile_description #

4. Now let’s build our new package:

cd ~/ros2_ws
colcon build

Check our new package. Open a new terminal window or source the .bashrc first in current:

source ~/.bashrc
ros2 pkg list

5. Create the following folders:

mkdir -p ~/ros2_ws/src/mec_mobile/mec_mobile_description/meshes/robot_3d/visual
mkdir -p ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/control
mkdir -p ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/mech
mkdir -p ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/sensors
mkdir -p ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/robots/

Add Meshe Files

Mesh files make your robot look realistic in robotics simulation and visualization programs. They visually represent the 3D shape of the robot parts, and are typically in formats such as STL (Stereo Lithography – .stl) or COLLADA (.dae).

The mesh files we use here were already available in this GitHub repository. However, if you want to create your own custom 3D printed robot from scratch, and make your own mesh files:

  • Design the robot’s 3D models using CAD programs like Onshape, Fusion 360, or Solidworks.

  • Export the 3D models as mesh files in formats like STL or COLLADA. These files contain information about the robot’s shape, including vertices, edges, and faces.

  • If needed, use a tool like Blender to simplify the mesh files. This makes them easier to use in simulations and visualizations.

  • Add the simplified mesh files to your URDF file to visually represent what the robot looks like.

Just pull these mesh files to corresponding directory of our package.

cd ~/ros2_ws/src/mec_mobile/mec_mobile_description/meshes

Configure CMakeLists.txt and package.xml

Let’s open Visual Studio Code and config the files for mec_mobile_description package.

cd ~/ros2_ws/src/mec_mobile/
code .
  1. Configure the CMakeLists.txt for the mec_mobile_description package:

CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(mec_mobile_description)
  
# Check if the compiler being used is GNU's C++ compiler (g++) or Clang.
# Add compiler flags for all targets that will be defined later in the 
# CMakeLists file. These flags enable extra warnings to help catch
# potential issues in the code.
# Add options to the compilation process
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()
  
# Locate and configure packages required by the project.
find_package(ament_cmake REQUIRED)
find_package(urdf_tutorial REQUIRED)
  
# Copy necessary files to designated locations in the project
install (
  DIRECTORY meshes urdf
  DESTINATION share/${PROJECT_NAME}
)
  
# Automates the process of setting up linting for the package, which
# is the process of running tools that analyze the code for potential
# errors, style issues, and other discrepancies that do not adhere to
# specified coding standards or best practices.
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()
  
ament_package()
  1. Make sure your package.xml for the mec_mobile_description package as follows:

package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>mec_mobile_description</name>
  <version>0.0.0</version>
  <description>Contains the robot description files that define the physical
    aspects of a robot, including its geometry, kinematics, dynamics, and
    visual aspects.</description>
  <maintainer email="robotlabs@todo.todo">ubuntu</maintainer>
  <license>BSD-3-Clause</license>
 
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>urdf_tutorial</depend>
 
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
 
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Build the Package

Before building the package, install dependencies:

cd ~/ros2_ws/
rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y

You should see: All required rosdeps installed successfully. If not, type your password, and install the required dependencies.

Now open a terminal window, and run colcon build:

colcon build

Add an alias (such as cb) for colcon build:

echo "alias cb='cd ~/dev_ws/ && colcon build && source ~/.bashrc'" >> ~/.bashrc

Create the URDF File

Now let’s create our URDF file. We will actually create it in XACRO format. XACRO files using macros and variables to simplify complex robot descriptions.

Before a ROS tool can use the XACRO file, it must be processed (translated) into a URDF file first, which allows for the dynamic generation of robot descriptions based on the XACRO file.

Open a terminal window, and type this command to create all the files we need:

touch ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/mech/{robot_3d_base.urdf.xacro,mecanum_wheel.urdf.xacro} ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/sensors/{rgbd_camera.urdf.xacro,imu.urdf.xacro,lidar.urdf.xacro} ~/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/robots/robot_3d.urdf.xacro

Robot Base

Let’s start with creating our base: robot_3d_base.urdf.xacro

robot_3d_base.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="total_width" value="0.19940" />
<xacro:property name="wheel_width" value="0.0304" />
<xacro:property name="wheel_radius" value="0.0325" />
<xacro:property name="total_height" value="0.26225" />
<xacro:property name="base_length" value="0.300" />
<xacro:property name="centered_length" value="0.238" />
<xacro:property name="base_x_offset" value="${(centered_length - base_length) / 2}" />
<xacro:property name="base_size_x" value="${base_length}" />
<xacro:property name="base_size_y" value="${total_width - (2 * wheel_width)}" />
<xacro:property name="base_size_z" value="${total_height - (2 * wheel_radius)}" />
<xacro:property name="base_mass" value="4.6" />

<xacro:macro name="robot_3d_base" params="prefix">
  <link name="${prefix}base_footprint"/>
  <link name="${prefix}base_link">
    <visual>
        <geometry>
            <mesh filename="file://$(find mec_mobile_description)/meshes/robot_3d/visual/base_link_X3.STL"/>
        </geometry>
        <material name="white">
          <color rgba="1 1 1 1"/>
        </material>
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </visual>
    <collision>
        <geometry>
            <box size="${base_size_x} ${base_size_y} ${base_size_z}"/>
        </geometry>
        <origin xyz="${base_x_offset} 0 ${base_size_z/2 + wheel_radius/4}" rpy="0 0 0"/>
    </collision>
    <inertial>
        <origin xyz="${base_x_offset} 0 ${base_size_z/2 + wheel_radius/4}" rpy="0 0 0"/>
        <mass value="${base_mass}"/>
        <inertia
            ixx="${(1/12) * base_mass * ((base_size_y * base_size_y) + (base_size_z * base_size_z))}" ixy="0.0" ixz="0.0"
            iyy="${(1/12) * base_mass * ((base_size_x * base_size_x) + (base_size_z * base_size_z))}" iyz="0.0"
            izz="${(1/12) * base_mass * ((base_size_x * base_size_x) + (base_size_y * base_size_y))}"/>
    </inertial>
  </link>
  <gazebo reference="${prefix}base_link">
    <visual>
        <material>
            <ambient>0 0.7 0 1</ambient>
            <diffuse>0 0.7 0 1</diffuse>
            <specular>0 0.7 0 1</specular>
        </material>
    </visual>
  </gazebo>
  <joint name="${prefix}base_joint" type="fixed">
    <parent link="${prefix}base_footprint"/>
    <child link="${prefix}base_link"/>
    <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
  </joint>
</xacro:macro>
</robot>

Robot Wheels

Then create a generic mecanum wheel: mecanum_wheel.urdf.xacro

mecanum_wheel.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="wheel_radius" value="0.0325" />
  <xacro:property name="wheel_separation" value="0.169" />
  <xacro:property name="wheel_width" value="0.03040" />
  <xacro:property name="wheel_mass" value="9.1" />
  <xacro:property name="wheel_xoff" value="0.08" />
  <xacro:property name="wheel_yoff" value="-0.01" />

  <xacro:macro name="mecanum_wheel" params="prefix side x_reflect y_reflect">
    <link name="${prefix}${side}_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="file://$(find mec_mobile_description)/meshes/robot_3d/visual/${side}_wheel_X3.STL"/>
        </geometry>
        <material name="dark_gray">
          <color rgba="0.2 0.2 0.2 1.0"/>
        </material>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>

      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${wheel_mass}"/>
        <inertia
          ixx="${(wheel_mass/12.0) * (3*wheel_radius*wheel_radius + wheel_width*wheel_width)}" ixy="0" ixz="0"
          iyy="${(wheel_mass/2.0) * (wheel_radius*wheel_radius)}" iyz="0"
          izz="${(wheel_mass/12.0) * (3*wheel_radius*wheel_radius + wheel_width*wheel_width)}"/>
      </inertial>
    </link>

    <joint name="${prefix}${side}_wheel_joint" type="continuous">
      <axis xyz="0 1 0"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}${side}_wheel_link"/>
      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(wheel_separation/2+wheel_yoff)} ${-wheel_radius}" rpy="0 0 0"/>
    </joint>

    <gazebo reference="${prefix}${side}_wheel_link">
      <surface>
        <friction>
          <ode>
              <mu>1.0</mu>
              <mu2>0.0</mu2>
          </ode>
        </friction>
      </surface>
      <visual>
          <material>
              <ambient>0.2 0.2 0.2 1.0</ambient>
              <diffuse>0.2 0.2 0.2 1.0</diffuse>
              <specular>0.2 0.2 0.2 1.0</specular>
              <emissive>0.0 0.0 0.0 0.0</emissive>
          </material>
      </visual>
  </gazebo>

  </xacro:macro>
</robot>

RGBD Camera

Create the RGBD camera: rgbd_camera.urdf.xacro. An RGBD camera that captures colors (RGB) information and measures depth (the D stands for depth). it allows the camera to create 3D maps of its surroundings.

rgbd_camera.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="M_PI" value="3.1415926535897931" />

  <xacro:macro name="rgbd_camera" params="
    prefix:=''
    camera_name:='cam_1'
    parent:='base_link'
    mesh_file:='file://$(find mec_mobile_description)/meshes/intel_realsense/visual/d435.stl'
    xyz_offset:='0.105 0 0.05'
    rpy_offset:='0 -0.50 0'
    mesh_xyz_offset:='0 0 0'
    mesh_rpy_offset:='${M_PI/2} 0.0 ${M_PI/2}'
    collision_xyz_offset:='0 0 0'
    collision_rpy_offset:='0 0 0'
    inertial_xyz_offset:='0 0 0'
    inertial_rpy_offset:='0 0 0'
    optical_xyz_offset:='0 0 0'
    optical_rpy_offset:='${-M_PI/2} 0 ${-M_PI/2}'
    depth_frame_xyz_offset:='0 0 0'
    depth_frame_rpy_offset:='0 0 0'
    infra1_xyz_offset:='0 0 0'
    infra1_rpy_offset:='0 0 0'
    infra2_xyz_offset:='0 -0.05 0'
    infra2_rpy_offset:='0 0 0'
    color_xyz_offset:='0 0.015 0'
    color_rpy_offset:='0 0 0'
    cam_width:=0.090
    cam_height:=0.025
    cam_depth:=0.02505
    mass:=0.072
    ixx:=0.003881243
    ixy:=0.0
    ixz:=0.0
    iyy:=0.000498940
    iyz:=0.0
    izz:=0.003879257
    material_name:='aluminum'
    material_color:='0.2 0.2 0.2 1'
    horizontal_fov:=1.5184
    image_width:=424
    image_height:=240
    clip_near:=0.05
    clip_far:=1.5
    update_rate:=2
    always_on:=1
    visualize:=true
    enable_collision:=false">

    <link name="${prefix}${camera_name}_link">
      <visual>
        <origin xyz="${mesh_xyz_offset}" rpy="${mesh_rpy_offset}"/>
        <geometry>
          <mesh filename="${mesh_file}" />
        </geometry>
        <material name="${material_name}">
          <color rgba="${material_color}"/>
        </material>
      </visual>
      <xacro:if value="${enable_collision}">
        <collision>
          <origin xyz="${collision_xyz_offset}" rpy="${collision_rpy_offset}"/>
          <geometry>
            <box size="${cam_depth} ${cam_width} ${cam_height}"/>
          </geometry>
        </collision>
      </xacro:if>
      <inertial>
        <mass value="${mass}" />
        <origin xyz="${inertial_xyz_offset}" rpy="${inertial_rpy_offset}"/>
        <inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}"
                 iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
      </inertial>
    </link>

    <joint name="${prefix}${camera_name}_joint" type="fixed">
      <parent link="${prefix}${parent}"/>
      <child link="${prefix}${camera_name}_link" />
      <origin xyz="${xyz_offset}" rpy="${rpy_offset}"/>
    </joint>

    <joint name="${prefix}${camera_name}_depth_joint" type="fixed">
      <origin xyz="${depth_frame_xyz_offset}" rpy="${depth_frame_rpy_offset}"/>
      <parent link="${prefix}${camera_name}_link"/>
      <child link="${prefix}${camera_name}_depth_frame" />
    </joint>
    <link name="${prefix}${camera_name}_depth_frame"/>

    <joint name="${prefix}${camera_name}_depth_optical_joint" type="fixed">
      <origin xyz="${optical_xyz_offset}" rpy="${optical_rpy_offset}" />
      <parent link="${prefix}${camera_name}_depth_frame" />
      <child link="${prefix}${camera_name}_depth_optical_frame" />
    </joint>
    <link name="${prefix}${camera_name}_depth_optical_frame"/>

    <joint name="${prefix}${camera_name}_infra1_joint" type="fixed">
      <origin xyz="${infra1_xyz_offset}" rpy="${infra1_rpy_offset}" />
      <parent link="${prefix}${camera_name}_link" />
      <child link="${prefix}${camera_name}_infra1_frame" />
    </joint>
    <link name="${prefix}${camera_name}_infra1_frame"/>

    <joint name="${prefix}${camera_name}_infra1_optical_joint" type="fixed">
      <origin xyz="${optical_xyz_offset}" rpy="${optical_rpy_offset}" />
      <parent link="${prefix}${camera_name}_infra1_frame" />
      <child link="${prefix}${camera_name}_infra1_optical_frame" />
    </joint>
    <link name="${prefix}${camera_name}_infra1_optical_frame"/>

    <joint name="${prefix}${camera_name}_infra2_joint" type="fixed">
      <origin xyz="${infra2_xyz_offset}" rpy="${infra2_rpy_offset}" />
      <parent link="${prefix}${camera_name}_link" />
      <child link="${prefix}${camera_name}_infra2_frame" />
    </joint>
    <link name="${prefix}${camera_name}_infra2_frame"/>

    <joint name="${prefix}${camera_name}_infra2_optical_joint" type="fixed">
      <origin xyz="${optical_xyz_offset}" rpy="${optical_rpy_offset}" />
      <parent link="${prefix}${camera_name}_infra2_frame" />
      <child link="${prefix}${camera_name}_infra2_optical_frame" />
    </joint>
    <link name="${prefix}${camera_name}_infra2_optical_frame"/>

    <joint name="${prefix}${camera_name}_color_joint" type="fixed">
      <origin xyz="${color_xyz_offset}" rpy="${color_rpy_offset}" />
      <parent link="${prefix}${camera_name}_link" />
      <child link="${prefix}${camera_name}_color_frame" />
    </joint>
    <link name="${prefix}${camera_name}_color_frame"/>

    <joint name="${prefix}${camera_name}_color_optical_joint" type="fixed">
      <origin xyz="${optical_xyz_offset}" rpy="${optical_rpy_offset}" />
      <parent link="${prefix}${camera_name}_color_frame" />
      <child link="${prefix}${camera_name}_color_optical_frame" />
    </joint>
    <link name="${prefix}${camera_name}_color_optical_frame"/>

    <gazebo reference="${prefix}${camera_name}_link">
      <sensor name="${prefix}${camera_name}" type="rgbd_camera">
        <camera>
          <horizontal_fov>${horizontal_fov}</horizontal_fov>
          <image>
            <width>${image_width}</width>
            <height>${image_height}</height>
          </image>
          <clip>
            <near>${clip_near}</near>
            <far>${clip_far}</far>
          </clip>
        </camera>
        <always_on>${always_on}</always_on>
        <gz_frame_id>${prefix}${camera_name}_link</gz_frame_id>
        <topic>${camera_name}</topic>
        <update_rate>${update_rate}</update_rate>
        <visualize>${visualize}</visualize>
      </sensor>
    </gazebo>

  </xacro:macro>
</robot>

The main camera link holds the visual 3D model (loaded from an STL file) and can optionally include other physical properties like collision geometry for simulation.

What makes RGBD links sophisticated is its handling of multiple camera frames:

  • Depth frame: Captures distance information

  • Infrared frames (infra1 and infra2): Used for stereo depth perception

  • Color frame: Regular RGB camera

Each frame is connected by fixed joints with specific offsets.

Robot LIDAR

lidar.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:property name="M_PI" value="3.1415926535897931" />

  <xacro:macro name="lidar_sensor" params="
    prefix:=''
    parent:='base_link'
    frame_id:='laser_frame'
    mesh_file:='file://$(find mec_mobile_description)/meshes/rplidar/rplidar_s2.stl'
    xyz_offset:='0 0 0.0825'
    rpy_offset:='0 0 0'
    mesh_xyz_offset:='0 0 0'
    mesh_rpy_offset:='${-M_PI/2} 0 0'
    mesh_scale:='0.001 0.001 0.001'
    collision_xyz_offset:='0 0 0'
    collision_rpy_offset:='0 0 0'
    inertial_xyz_offset:='0 0 0'
    inertial_rpy_offset:='0 0 0'
    lidar_width:=0.077
    lidar_height:=0.0398
    mass:=0.185
    material_name:='rplidar_black'
    material_color:='0.1 0.1 0.1 1'
    gazebo_material_ambient:='0.1 0.1 0.1 1'
    gazebo_material_diffuse:='0.1 0.1 0.1 1'
    gazebo_material_specular:='0.1 0.1 0.1 1'
    update_rate:=10
    ray_count:=720
    min_angle:='-3.14159'
    max_angle:='3.14159'
    min_range:='0.20'
    max_range:='30.0'
    range_resolution:='0.013'
    topic_name:='scan'
    always_on:=true
    visualize:=false
    enable_collision:=false">

    <xacro:property name="radius" value="${lidar_width/2}" />
    <xacro:property name="ixx_iyy" value="${(mass/12) * (3 * radius * radius + lidar_height * lidar_height)}" />
    <xacro:property name="izz" value="${(mass/2) * (radius * radius)}" />

    <link name="${prefix}${frame_id}">
      <visual>
        <origin xyz="${mesh_xyz_offset}" rpy="${mesh_rpy_offset}"/>
        <geometry>
          <mesh filename="${mesh_file}" scale="${mesh_scale}"/>
        </geometry>
        <material name="${material_name}">
          <color rgba="${material_color}"/>
        </material>
      </visual>

      <xacro:if value="${enable_collision}">
        <collision>
          <origin xyz="${collision_xyz_offset}" rpy="${collision_rpy_offset}"/>
          <geometry>
            <cylinder radius="${lidar_width/2}" length="${lidar_height}"/>
          </geometry>
        </collision>
      </xacro:if>

      <inertial>
        <mass value="${mass}" />
        <origin xyz="${inertial_xyz_offset}" rpy="${inertial_rpy_offset}"/>
        <inertia
          ixx="${ixx_iyy}"
          ixy="0.0"
          ixz="0.0"
          iyy="${ixx_iyy}"
          iyz="0.0"
          izz="${izz}" />
      </inertial>
    </link>

    <joint name="${prefix}${frame_id}_joint" type="fixed">
      <parent link="${prefix}${parent}"/>
      <child link="${prefix}${frame_id}" />
      <origin xyz="${xyz_offset}" rpy="${rpy_offset}"/>
    </joint>

    <gazebo reference="${prefix}${frame_id}">
      <material>
        <ambient>${gazebo_material_ambient}</ambient>
        <diffuse>${gazebo_material_diffuse}</diffuse>
        <specular>${gazebo_material_specular}</specular>
      </material>
      <sensor name="${prefix}lidar_sensor" type="gpu_lidar">
        <topic>${topic_name}</topic>
        <update_rate>${update_rate}</update_rate>
        <always_on>${always_on}</always_on>
        <visualize>${visualize}</visualize>
        <ray>
          <scan>
            <horizontal>
              <samples>${ray_count}</samples>
              <resolution>1</resolution>
              <min_angle>${min_angle}</min_angle>
              <max_angle>${max_angle}</max_angle>
            </horizontal>
            <vertical>
              <samples>1</samples>
              <resolution>1</resolution>
              <min_angle>0</min_angle>
              <max_angle>0</max_angle>
            </vertical>
          </scan>
          <range>
            <min>${min_range}</min>
            <max>${max_range}</max>
            <resolution>${range_resolution}</resolution>
          </range>
        </ray>
        <gz_frame_id>${prefix}${frame_id}</gz_frame_id>
      </sensor>
    </gazebo>
  </xacro:macro>
</robot>

Robot IMU

imu.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:property name="M_PI" value="3.1415926535897931" />

    <xacro:macro name="imu_sensor" params="
        prefix:=''
        parent:='base_link'
        frame_id:='imu'
        xyz_offset:='0 0 0'
        rpy_offset:='0 0 0'
        inertial_xyz_offset:='0 0 0'
        inertial_rpy_offset:='0 0 0'
        mass:=0.031
        length:=0.03899
        width:=0.03785
        height:=0.01338
        material_name:='imu_black'
        material_color:='0.1 0.1 0.1 1'
        gazebo_material_ambient:='0.1 0.1 0.1 1'
        gazebo_material_diffuse:='0.1 0.1 0.1 1'
        gazebo_material_specular:='0.1 0.1 0.1 1'
        update_rate:='15.0'
        topic_name:='imu/data'
        always_on:=true
        visualize:=false">

        <xacro:property name="ixx" value="${(mass/12.0) * (height*height + width*width)}" />
        <xacro:property name="iyy" value="${(mass/12.0) * (length*length + height*height)}" />
        <xacro:property name="izz" value="${(mass/12.0) * (length*length + width*width)}" />

        <link name="${prefix}${frame_id}_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <box size="${length} ${width} ${height}"/>
                </geometry>
                <material name="${material_name}">
                    <color rgba="${material_color}"/>
                </material>
            </visual>

            <inertial>
                <mass value="${mass}" />
                <origin xyz="${inertial_xyz_offset}" rpy="${inertial_rpy_offset}"/>
                <inertia
                    ixx="${ixx}"
                    ixy="0.0"
                    ixz="0.0"
                    iyy="${iyy}"
                    iyz="0.0"
                    izz="${izz}" />
            </inertial>
        </link>

        <joint name="${prefix}${frame_id}_joint" type="fixed">
            <parent link="${prefix}${parent}"/>
            <child link="${prefix}${frame_id}_link" />
            <origin xyz="${xyz_offset}" rpy="${rpy_offset}"/>
        </joint>

        <gazebo reference="${prefix}${frame_id}_link">
            <material>
                <ambient>${gazebo_material_ambient}</ambient>
                <diffuse>${gazebo_material_diffuse}</diffuse>
                <specular>${gazebo_material_specular}</specular>
            </material>
            <sensor name="${prefix}imu_sensor" type="imu">
                <topic>${topic_name}</topic>
                <update_rate>${update_rate}</update_rate>
                <always_on>${always_on}</always_on>
                <visualize>${visualize}</visualize>
                <gz_frame_id>${prefix}${frame_id}_link</gz_frame_id>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>

An IMU (Inertial Measurement Unit) is a sensor that measures movement, specifically acceleration, rotation, and sometimes magnetic fields, to help determine an object’s position and motion.

Full Robot

Finally, let’s create the full robot, bringing together all the components we have created:

robot_3d.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot_3d">

    <xacro:property name="M_PI" value="3.1415926535897931" />

    <xacro:arg name="robot_name" default="robot_3d"/>
    <xacro:arg name="prefix" default=""/>
    <xacro:arg name="use_gazebo" default="false"/>

    <xacro:include filename="$(find mec_mobile_description)/urdf/mech/robot_3d_base.urdf.xacro"/>
    <xacro:include filename="$(find mec_mobile_description)/urdf/mech/mecanum_wheel.urdf.xacro"/>
    <xacro:include filename="$(find mec_mobile_description)/urdf/sensors/rgbd_camera.urdf.xacro"/>
    <xacro:include filename="$(find mec_mobile_description)/urdf/sensors/lidar.urdf.xacro"/>
    <xacro:include filename="$(find mec_mobile_description)/urdf/sensors/imu.urdf.xacro"/>

    <xacro:robot_3d_base prefix="$(arg prefix)"/>

    <xacro:mecanum_wheel
      prefix="$(arg prefix)"
      side="front_left"
      x_reflect="1"
      y_reflect="1"/>

    <xacro:mecanum_wheel
      prefix="$(arg prefix)"
      side="front_right"
      x_reflect="1"
      y_reflect="-1"/>

    <xacro:mecanum_wheel
      prefix="$(arg prefix)"
      side="back_left"
      x_reflect="-1"
      y_reflect="1"/>

    <xacro:mecanum_wheel
      prefix="$(arg prefix)"
      side="back_right"
      x_reflect="-1"
      y_reflect="-1"/>

    <xacro:rgbd_camera
      prefix="$(arg prefix)"
      camera_name="cam_1"
      xyz_offset="0.105 0 0.03"
      rpy_offset="0 0 0"/>

    <xacro:lidar_sensor
      prefix="$(arg prefix)"
      parent="base_link"
      frame_id="laser_frame"
      xyz_offset="0 0 0.0825"
      rpy_offset="0 0 3.14"
      mesh_xyz_offset="0 0 0"
      mesh_rpy_offset="${-M_PI/2} 0 0"
      topic_name="scan"/>

    <xacro:imu_sensor
      prefix="$(arg prefix)"
      parent="base_link"
      frame_id="imu"
      xyz_offset="0 0 0.006"
      rpy_offset="0 0 0"
      update_rate="15.0"
      topic_name="imu/data"/>
    <!--
    <xacro:include filename="$(find mec_mobile_description)/urdf/control/velocity_control_plugin.urdf.xacro" />
    <xacro:load_velocity_control_plugin use_gazebo="$(arg use_gazebo)"/>

    <xacro:include filename="$(find mec_mobile_description)/urdf/control/gazebo_sim_ros2_control.urdf.xacro" />
    <xacro:load_gazebo_sim_ros2_control_plugin
      robot_name="$(arg robot_name)"
      use_gazebo="$(arg use_gazebo)"/>

    <xacro:include filename="$(find mec_mobile_description)/urdf/control/robot_3d_ros2_control.urdf.xacro" />
    <xacro:mec_mobile_ros2_control
      prefix="$(arg prefix)"
      use_gazebo="$(arg use_gazebo)"/>
    -->
    
</robot>

URDF Explanation

Walk through each URDF file

Build the Package

Now let’s build the package.

cd ~/ros2_ws
colcon build
source ~/ros2_ws/install/setup.bash

Visualize the URDF in RViz

Let’s visualize the URDF files in RViz.

RViz (short for “ROS Visualization”) is a 3D visualization tool for robots that allows you to view the robot’s sensors, environment, and state. I use it all the time to visualize and debug my robots.

Now launch the URDF files. The conversion from XACRO to URDF happens behind the scenes. Be sure to have the correct path to your XACRO file.

ros2 launch urdf_tutorial display.launch.py model:=/home/robotlabs/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/robots/robot_3d.urdf.xacro
  1. Make sure your have installed urdf-tutorial package, if not, run following command. sudo apt-get install ros-${ROS_DISTRO}-urdf-tutorial

  2. model:=/home/robotlabs/ros2_ws/src/mec_mobile/mec_mobile_description... Replace this directory according to your own project, like workspace name etc.

By convention, the red axis is the x-axis, the green axis in the y-axis, and the blue axis is the z-axis. Uncheck the TF checkbox to turn off the axes.

You can use the Joint State Publisher GUI pop-up window to move the links around.

On the left panel under Displays, play around by checking and unchecking different options.

  1. You can also see what simulation engines will use to detect collisions: Uncheck “Visual Enabled” under Robot Model and check “Collision Enabled.”

  2. Under Robot Model, you can see how the mass distribution by unchecking “Visual Enabled” and “Collision Enabled” and checking the “Mass” checkbox under “Mass Properties”.

You can also see the coordinate frames. Open a new terminal window, and type the following commands:

cd ~/Documents/
ros2 run tf2_tools view_frames

To see the coordinate frames, type:

dir
evince frames_YYYY-MM-DD_HH.MM.SS.pdf
TF Tree

Another useful tool of ROS is the TF Tree. This tool helps visualizing the transformations between the reference frames of the robot. First we need to install the tool:

sudo apt install ros-jazzy-rqt-tf-tree 

After that, let's view our robot with the previous command in RViz:

ros2 launch urdf_tutorial display.launch.py model:=/home/robotlabs/ros2_ws/src/mec_mobile/mec_mobile_description/urdf/robots/robot_3d.urdf.xacro

and in another terminal let's run TF Tree:

ros2 run rqt_tf_tree rqt_tf_tree

You might experience an issue during the first start of TF Tree, in this case make sure that this rqt plugin is discovered:

ros2 run rqt_tf_tree rqt_tf_tree --force-discove

The official tutorial for creating a URDF file is . The URDF file gives the robot a digital body that software can interact with, and it allows software tools to understand the robot’s structure, enabling tasks like simulation, motion planning, and sensor data interpretation etc.

You have a Ubuntu 24.04 PC or VM with ROS2 Jazzy and Gazebo installed, if not, check our .

You can find the code here on .

If you want to save the code on Github, .

A metapackage doesn’t contain anything except a list of dependencies to other packages. You can use a metapackage to make it easier to install multiple related packages at once. .

Optical frames: Aligned with

You can find heaps of sensors that can be implemented in Gazebo simulator in .

Create the : lidar.urdf.xacro

We will add the so we can generate simulated LIDAR data in a future tutorial.

Now let’s : imu.urdf.xacro

here on the ROS 2 website
previous tutorials
GitHub
check our previous video on Youtube
Example
standard ROS conventions
this GitHub repository
LIDAR
LIDAR plugin
create the IMU