ROS2 Tutorial: Watch Your Robot Vehicle Move for the First Time! (Part 3)

In the previous blogs, we initially defined a robot vehicle in URDF and then defined the same vehicle using Xacros by creating templates and reusing them. In this last blog of the series (I guess), we are going to add motion to that vehicle and make it run in the virtual map. In the end, the vehicle will move like this:

So let’s get started:

Defining the Transformation

[Before starting, note that this blog is longer than usual and uses an external ROS2 package as dependency. If you happen to get stuck somewhere, feel free to text me on Instagram @machinelearningsite before frying up your brains on some error.]

In the previous post of this series, we create my_vehicle.xacro that defines the structure of the vehicle. We created xacros of chassis and wheels and created instances of them that ultimately gave rise to the entire vehicle. The same xacro file is extended with ROS2 control interface and transmission interface that allows us to create transformation from the vehicle coordinate frame to the world frame. Below is what the end file looks like: (The my_vehicle.xacro should be present in the config/ folder of urdf_launch package. If what I just said made your brain buffer, it means you skipped the previous post in this series. So click here to know what I am talking about.)

<?xml version="1.0"?>
<!--
  vehicle.xacro
  A simple example of using Xacro to create a vehicle-like robot
  with a chassis and four wheels.
-->

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="xacro_vehicle">

  <!-- ========== Parameters ========== -->
  <!-- You can tweak these at the top to reshape your robot -->
  <xacro:property name="chassis_length" value="1.0"/>
  <xacro:property name="chassis_width"  value="0.6"/>
  <xacro:property name="chassis_height" value="0.2"/>
  <xacro:property name="wheel_radius"   value="0.15"/>
  <xacro:property name="wheel_width"    value="0.05"/>

  <!-- ========== Macros ========== -->
  <!-- Chassis macro -->
  <xacro:macro name="chassis" params="name color">
    <link name="${name}">
      <visual>
        <geometry>
          <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
        </geometry>
        <material name="${color}">
          <color rgba="0.2 0.2 0.8 1"/>
        </material>
      </visual>
    </link>
  </xacro:macro>

  <!-- Wheel macro -->
  <xacro:macro name="wheel" params="name parent xyz color">
    <link name="${name}">
      <visual>
        <geometry>
          <cylinder length="${wheel_width}" radius="${wheel_radius}"/>
        </geometry>
        <material name="${color}">
          <color rgba="0.1 0.1 0.1 1"/>
        </material>
        <origin rpy="1.5708 0 0"/> <!-- rotate to lay flat -->
      </visual>

      <inertial>
        <mass value="1.0"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="0.1" iyy="0.1" izz="0.1" ixy="0.0" ixz="0.0" iyz="0.0"/>
      </inertial>
    </link>

    <joint name="joint_${name}" type="continuous">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <origin xyz="${xyz}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/> <!-- wheel spins around the y-axis -->
    </joint>
  </xacro:macro>

  <!-- ========== Robot Structure ========== -->
  <!-- Root link -->
  <xacro:chassis name="base_link" color="blue"/>

  <!-- Wheels (x: forward/back, y: left/right, z: height) -->
  <xacro:wheel name="front_left_wheel"  parent="base_link" xyz="0.35  0.25 -0.15" color="black"/>
  <xacro:wheel name="front_right_wheel" parent="base_link" xyz="0.35 -0.25 -0.15" color="black"/>
  <xacro:wheel name="rear_left_wheel"   parent="base_link" xyz="-0.35  0.25 -0.15" color="black"/>
  <xacro:wheel name="rear_right_wheel"  parent="base_link" xyz="-0.35 -0.25 -0.15" color="black"/>

    <!-- ========== ROS 2 Control Setup ========== -->
  <ros2_control name="VehicleSystem" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/DiffBotSystemHardware</plugin>
    </hardware>

    <joint name="joint_rear_left_wheel">
      <command_interface name="velocity"/>
      <state_interface name="velocity"/>
      <state_interface name="position"/>
    </joint>

    <joint name="joint_rear_right_wheel">
      <command_interface name="velocity"/>
      <state_interface name="velocity"/>
      <state_interface name="position"/>
    </joint>
  </ros2_control>

  <!-- Transmission interfaces -->
  <transmission name="rear_left_wheel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_rear_left_wheel">
      <hardware_interface>hardware_interface/VelocityJointInterface</hardware_interface>
    </joint>
    <actuator name="rear_left_motor">
      <hardware_interface>hardware_interface/VelocityJointInterface</hardware_interface>
      <mechanical_reduction>1</mechanical_reduction>
    </actuator>
  </transmission>

  <transmission name="rear_right_wheel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_rear_right_wheel">
      <hardware_interface>hardware_interface/VelocityJointInterface</hardware_interface>
    </joint>
    <actuator name="rear_right_motor">
      <hardware_interface>hardware_interface/VelocityJointInterface</hardware_interface>
      <mechanical_reduction>1</mechanical_reduction>
    </actuator>
  </transmission>


</robot>

The <ros2_control> tag defines how the robot interfaces with ROS 2’s controller system. Inside it, we declare a hardware plugin, in this case ros2_control_demo_hardware/DiffBotSystemHardware. This plugin acts as a mock hardware interface: it pretends to be the robot’s motor driver, allowing us to test motion without real hardware. Essentially, it gives ROS 2 a way to send and receive commands for our simulated joints.

Next, each joint that we want to control is listed explicitly. For our vehicle, the two rear wheel joints have command and state interfaces for velocity and position. This tells ROS 2 that these joints can receive velocity commands (to spin the wheels) and can also report their current state back. When we later load a controller—like the diff_drive_controller—these interfaces are what it will use to make the robot move forward, backward, or turn.

Finally, the <transmission> elements describe how each actuator connects to its corresponding joint. Think of a transmission as a bridge between the control system and the physical motion. Here we use a simple one-to-one mapping through SimpleTransmission, meaning that one unit of motor rotation equals one unit of wheel rotation. The mechanical reduction is set to 1, so there’s no gearing or scaling involved.

The next step will be to bring in a controller plugin that issues real commands—finally making the robot roll.

Writing the Vehicle Interface Node

In your ROS2 workspace, create a new package and add the required dependencies to it:

cd ~/ros2_ws/src/
ros2 pkg create --build-type ament_cmake --license Apache-2.0 demo_vehicle_interface --dependencies rclcpp geometry_msgs tf2_ros tf2_geometry_msgs sensor_msgs

Create the demo_vehicle_interface_node.hpp and demo_vehicle_interface_node.cpp files in include/demo_vehicle_interface/ and src/ folders respectively.

Let us start by creating the header file demo_vehicle_interface_node.hpp:

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <cmath>
#include <thread>
#include <chrono>

using namespace std::chrono;

class VehicleInterface : public rclcpp::Node
{
    public:
        explicit VehicleInterface(rclcpp::NodeOptions options = rclcpp::NodeOptions());

    private:
        rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
        std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster;
        rclcpp::Rate::SharedPtr loop_rate_;
        rclcpp::TimerBase::SharedPtr timer_;

        const double degree = M_PI/180.0;
        double joint_front_left_wheel = 0.;
        double joint_front_right_wheel = 0.;
        double joint_rear_left_wheel = 0.;
        double joint_rear_right_wheel = 0.;
        
        void publish();
};

Here’s a brief description of what’s going on. The class VehicleInterface inherits from rclcpp::Node, making it a fully functional ROS 2 node. Inside, it sets up a publisher for sensor_msgs::msg::JointState, which regularly sends out the current rotation angles of each wheel. It also creates a tf2_ros::TransformBroadcaster, responsible for broadcasting coordinate frame information—essentially defining the movement of one coordinate frame is moving relative to another in ROS 2. At runtime, the node repeatedly calls its publish() function at a fixed rate. This function updates the joint states, computes their transforms, and publishes them on the appropriate ROS topics.

And now demo_vehicle_interface_node.cpp:

#include "demo_vehicle_interface/demo_vehicle_interface_node.hpp"


VehicleInterface::VehicleInterface(rclcpp::NodeOptions options): Node("demo_vehicle_interface_pubisher",options)
{
    joint_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
    // create a publisher to tell robot_state_publisher the JointState information.
    // robot_state_publisher will deal with this transformation
    broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    // create a broadcaster to tell the tf2 state information
    // this broadcaster will determine the position of coordinate system 'axis' in coordinate system 'odom'
    RCLCPP_INFO(this->get_logger(),"Starting state publisher");
    loop_rate_=std::make_shared<rclcpp::Rate>(33ms);
    timer_=this->create_wall_timer(33ms,std::bind(&VehicleInterface::publish,this));
}

void VehicleInterface::publish()
{
    // create the necessary messages
    geometry_msgs::msg::TransformStamped t;
    sensor_msgs::msg::JointState joint_state;

    // add time stamp
    joint_state.header.stamp=this->get_clock()->now();
    // Specify joints' name which are defined in the r2d2.urdf.xml and their content
    joint_state.name={"joint_front_left_wheel","joint_front_right_wheel","joint_rear_left_wheel","joint_rear_right_wheel"};
    joint_state.position={joint_front_left_wheel, joint_front_right_wheel, joint_rear_left_wheel, joint_rear_right_wheel};

    // add time stamp
    t.header.stamp=this->get_clock()->now();
    // specify the father and child frame

    // odom is the base coordinate system of tf2
    t.header.frame_id="map";
    // axis is defined in r2d2.urdf.xml file and it is the base coordinate of model
    t.child_frame_id="base_link";

    // add translation change
    t.transform.translation.x=cos(joint_rear_left_wheel)*2;
    t.transform.translation.y=sin(joint_rear_left_wheel)*2;
    t.transform.translation.z=0.3;
    tf2::Quaternion q;
    // euler angle into Quanternion and add rotation change
    q.setRPY(0,0,joint_rear_left_wheel+M_PI/2);
    t.transform.rotation.x=q.x();
    t.transform.rotation.y=q.y();
    t.transform.rotation.z=q.z();
    t.transform.rotation.w=q.w();

    joint_rear_left_wheel+=degree;  // Increment by 1 degree (in radians)
    joint_rear_right_wheel+=degree;    // Change angle at a slower pace

    // send message
    broadcaster->sendTransform(t);
    joint_pub_->publish(joint_state);

    RCLCPP_INFO(this->get_logger(),"Publishing joint state");
}

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<VehicleInterface>());
    rclcpp::shutdown();
    return 0;
}

Now some info about the code in the .cpp file. When the node is initialized, it creates two key communication channels: a publisher for joint states and a transform broadcaster. The joint state publisher continuously sends information about how much each wheel has rotated, while the broadcaster announces the robot’s position and orientation in space through the tf2 framework.

Inside the constructor, a timer is set up to repeatedly call the publish() function at a steady rate. This creates a consistent loop where new joint and transform data are generated and published about 33 times per second. This can be varied and set to whatever is suitable. The log message confirms when the node starts, which is helpful for debugging.

The publish() function is the heart of this node. Every time it runs, it prepares two ROS messages: one for joint states and one for coordinate transforms. The joint state message updates the rotation angles for each wheel, mimicking motion. The transform message defines the robot’s position (map → base_link) and orientation in 3D space. The position is updated slightly each cycle using simple trigonometric functions, making the robot appear to move forward in a circular path. Finally, both messages are sent out—one to the tf2 tree and one to the joint_states topic.

Next, the launch file brings all the pieces of the robot together — the robot_state_publisher, the freshly created demo_vehicle_interface, and the urdf_launch — into a single running system. (Visit this first post of this series if you don’t have urdf_launch package in your workspace).

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import FileContent, LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch_ros.parameter_descriptions import ParameterValue

def generate_launch_description():
    # ''use_sim_time'' is used to have ros2 use /clock topic for the time source
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
 
    xacro_file = FileContent(
        PathJoinSubstitution([FindPackageShare('urdf_tutorial'), 'urdf', 'my_vehicle.xacro']))
    
    
    robot_description = ParameterValue(
        Command(['xacro', xacro_file]),
        value_type=str
    )

    urdf_launch = IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('urdf_launch'), 'launch', 'display.launch.py']),
            launch_arguments={
                'urdf_package': 'urdf_tutorial',
                'urdf_package_path': PathJoinSubstitution(['urdf', 'my_vehicle.urdf'])
            }.items()
        )
    
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false'),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time, 'robot_description': "/home/cmodi/cmodi/ros2_ws/src/mls_ros2_packages/xacro_tutorial/urdf_tutorial/urdf/my_vehicle.xacro"}],
            ),
        Node(
            package='demo_vehicle_interface',
            executable='demo_vehicle_interface_node',
            name='demo_vehicle_interface',
            output='screen'),
        urdf_launch,
    ])

So here’s a small explanation of why we need to start these nodes. The robot_state_publisher node reads the robot’s description and publishes the transforms for each joint and link, effectively building the robot’s structure in the ROS 2 transform tree. Then, the demo_vehicle_interface node starts — this is the node that continuously publishes joint and motion updates, making the robot appear to move. Finally, the file includes another launch file from the urdf_launch package, which handles visualization (usually opening RViz so you can see the robot model in action).

Configuring CMakeLists.txt and package.xml files

Ultimately comes the most boring part when a new ROS2 node is created: Configuring the the CMakeLists.txt and package.xml. Edit your CMakeLists.txt to make it look like this:

cmake_minimum_required(VERSION 3.8)
project(demo_vehicle_interface)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(ament_cmake_auto REQUIRED)

include_directories(
  include
  ${rclcpp_INCLUDE_DIRS}
)

ament_auto_find_build_dependencies()

add_executable(demo_vehicle_interface_node src/demo_vehicle_interface_node.cpp)

target_include_directories(demo_vehicle_interface_node PUBLIC include)

ament_target_dependencies(demo_vehicle_interface_node
  geometry_msgs
  sensor_msgs
  tf2_ros
  tf2_geometry_msgs
  rclcpp
)

install(TARGETS
  demo_vehicle_interface_node
  DESTINATION lib/${PROJECT_NAME}
)


install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}
)

ament_auto_package(INSTALL_TO_SHARE
  launch
)

Your package.xml should already contain the dependencies if they were mentioned while creating the package but have a look to verify:

<?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>demo_vehicle_interface</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="kolbenkraft.kk@gmail.com">MLS</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>geometry_msgs</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>sensor_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Results

PHEW! Too many lines of code. But now it is done. Just build your node and launch it:

cd ~/ros2_ws
colcon build --packages-up-to demo_vehicle_interface urdf_tutorial
source install/setup.bash
ros2 launch demo_vehicle_interface demo_vehicle_interface.launch.py

In the RViz window, make sure to set the Fixed Frame to “map”. You will find this under the Global Options tab in the top left corner of the RViz window. If all works well, your vehicle should be runnning as in this video:

What’s Next

I started this series to explain how to design and create a robot vehicle in ROS2 and use it to simulate the vehicle movements. In this era, autonomous mobility is a hot topic and a lot of effort and money is put into its research. ROS2 helps in simulating the vehicle body and its maneuvers and allows us to test different systems and logic for controlling it. A perfect example of this is a PID controller that controls the vehicle’s acceleration or braking. Another example is teleoperated driving where a vehicle is controlled remotely using a controller. ROS2 provides a perfect platform to monitor the input commands and the corresponding output.

If you enjoyed this and are more interested in self-driving cars and autonomous driving, check out my post Implementing Bicycle Model: A Fundamental Concept in Self Driving Vehicles that focuses on the concept of vehicle dynamics that serves as a fundamental topic in designing self-driving cars. Here’s the introductory excerpt from the blog:

“In the development of self driving vehicles, a cornerstone concept that engineers rely on is the “bicycle model.” This abstract representation simplifies the complex dynamics of a car into a more manageable framework that resembles that of a bicycle. By breaking down the vehicle’s motion into lateral dynamics, the bicycle model offers a structured approach to predicting and controlling car behavior.

In this blog, I will provide Python implementation of this model to demonstrate its significance in understanding the fundamentals of self driving vehicles.”

If you happen to have any other issues, some new idea, a blog suggestion or saying want to drop a “hi”, feel free to ping me on Instagram @machinelearningsite. Till then, keep coding and have fun building stuff. Peace!

Tags: , , , ,

Leave a Reply