In this tutorial, we will explore how to use the rqt_console tool in ROS 2 to analyze and debug your robotic system. The rqt_console is a graphical user interface that displays log messages from various nodes in your ROS 2 system, making it easier to identify and diagnose issues.
Sure, you could run your program and scroll through the terminal window to see all the logs. But rqt_console makes that process a lot more efficient, enabling you to filter for exactly what you are looking for.
In this tutorial, we will simulate and control a mobile robot in Gazebo. I will show you how to set up and control a mecanum wheel robot using ROS 2 Control and Gazebo.
By the end of this tutorial, you will be able to build this:
If you prefer to learn by video, you can follow the entire tutorial here:
We need to create a controller for a mobile robot with mecanum omnidirectional wheels since none exists in the official ros2_controllers package. The official instructions for creating a new controller are on this page. We will adapt these instructions to your use case.
Before we write our source code, let’s dive into the mathematics that make omnidirectional movement possible. This foundation will be important when we implement our controller in the next section.
The coordinate system for our robot follows ROS 2 conventions. The base frame’s X-axis points forward, Y-axis points left, and rotations follow the right-hand rule with positive counterclockwise rotation. We number our wheels starting from the front-left as Wheel 1 (FL), moving clockwise to Wheel 2 (FR), Wheel 3 (RL), and Wheel 4 (RR).
Understanding Wheel Rotation Direction
Before diving into the kinematics equations, we need to understand how wheel rotations are defined in our system.
Each wheel’s rotation axis aligns with its positive y-axis, which runs parallel to the robot base’s y-axis. You can see this in the mecanum_wheel.urdf.xacro file. A positive wheel velocity value indicates counterclockwise rotation when viewed down this y-axis (imagine looking at the wheel from its side with the y-axis pointing towards you).
This convention has practical implications. For example, when commanding the robot to move forward, each wheel rotates counterclockwise around its y-axis, creating a positive velocity value. Think of the wheel’s 2D plane (formed by its positive z and x axes) rotating counterclockwise around the y-axis. This coordinated motion drives the robot base forward.
Forward Kinematics: From Wheel Speeds to Robot Motion
The forward kinematics equations tell us how individual wheel velocities combine to create the robot’s overall motion. These equations are particularly useful when we need to estimate the robot’s actual movement based on wheel encoder feedback.
Looking at the equations, we can see how each wheel contributes to the robot’s linear velocities (vx and vy) and angular velocity (ωz). The wheel radius R scales the contribution of each wheel’s angular velocity (ω1 through ω4).
Inverse Kinematics for Motion Control
In our ROS 2 controller, we’ll primarily use inverse kinematics. These equations help us calculate the required wheel velocities to achieve desired robot motion. When we receive velocity commands (typically from the /cmd_vel topic), we’ll use these equations to determine appropriate wheel speeds.
The inverse kinematics equations consider:
The robot’s desired linear velocities (vx and vy)
The desired angular velocity (ωz)
The robot’s physical dimensions (L and W)
The wheel radius (R)
In our mecanum drive controller we transform velocity commands into actual wheel motions. We also handle odometry feedback using the forward kinematics equations to provide accurate motion estimation of the full robot.
Mecanum Wheel Kinematics in ROS 2: Translating Equations into Code
Now that we understand the mathematics behind forward and inverse kinematics, let’s map these equations to ROS 2 concepts. This step bridges the gap between theory and implementation, ensuring a smooth transition into developing a functional mecanum drive controller.
Forward Kinematics in ROS 2
These equations estimate the robot’s motion based on wheel encoder feedback, providing odometry as linear and angular velocities in meters per second and radians per second.
Inverse Kinematics in ROS 2
These equations allow us to translate velocity commands into individual wheel speeds.
Create Source Files
Now let’s create the source files.
cd ~/ros2_ws/src/yahboom_rosmaster/mecanum_drive_controller/
Finally, let’s run the tests to ensure our controller can be found and loaded:
colcon test --packages-select mecanum_drive_controller
OR (for detailed output)
colcon test --packages-select mecanum_drive_controller --event-handlers console_direct+
You can also see your new controller if you type:
ros2 pkg list
Connect Gazebo to ROS 2 Control
Create a YAML Configuration File for the Controller Manager
After setting up our simulation environment in Gazebo, we need to tell ROS 2 Control how to manage our robot’s movements in this virtual world. Let’s create a configuration file that defines how our robot’s wheels should be controlled.
Open a terminal window, and type:
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_description/
mkdir -p config/rosmaster_x3/
Create a new file inside the config/rosmaster_x3 directory called:
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_description/launch
Save the file, and close it.
Create the XACRO/URDF Files
Next, we need to add three essential XACRO/URDF files that bridge Gazebo simulation with ROS 2 Control. These files work alongside your controller configuration YAML file to create a complete control system.
gazebo_sim_ros2_control.urdf.xacro
Open a terminal window.
Type the following command:
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_description/urdf/
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_bringup/
mkdir launch
cd launch
Add this code called load_ros2_controllers.launch.py:
#!/usr/bin/env python3
"""
Launch ROS 2 controllers for the mecanum wheel robot.
This script creates a launch description that starts the necessary controllers
for operating the mecanum wheel robot in a specific sequence.
Launched Controllers:
1. Joint State Broadcaster: Publishes joint states to /joint_states
2. Mecanum Drive Controller: Controls the robot's mecanum drive movements via ~/cmd_vel
:author: Addison Sears-Collins
:date: November 20, 2024
"""
from launch import LaunchDescription
from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessExit
def generate_launch_description():
"""Generate a launch description for sequentially starting robot controllers.
The function creates a launch sequence that ensures controllers are started
in the correct order.
Returns:
LaunchDescription: Launch description containing sequenced controller starts
"""
# Start mecanum drive controller
start_mecanum_drive_controller_cmd = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'mecanum_drive_controller'],
output='screen'
)
# Start joint state broadcaster
start_joint_state_broadcaster_cmd = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'joint_state_broadcaster'],
output='screen'
)
# Add delay to joint state broadcaster
delayed_start = TimerAction(
period=20.0,
actions=[start_joint_state_broadcaster_cmd]
)
# Register event handler for sequencing
load_joint_state_broadcaster_cmd = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=start_joint_state_broadcaster_cmd,
on_exit=[start_mecanum_drive_controller_cmd]))
# Create and populate the launch description
ld = LaunchDescription()
# Add the actions to the launch description in sequence
ld.add_action(delayed_start)
ld.add_action(load_joint_state_broadcaster_cmd)
return ld
Save the file, and close it.
Update CMakeLists.txt for the yahboom_rosmaster_bringup package to include the new launch folder. Make sure the block below, looks like this:
# Copy necessary files to designated locations in the project
install (
DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
Create YAML File for ROS 2 Bridge
We now need to create a YAML file that bridges between ROS 2 and Gazebo topics. This file will specify how sensor data (camera info, point clouds, IMU data, and LIDAR data) should be translated between the Gazebo simulation environment and the ROS 2 ecosystem. The configuration ensures that simulated sensor data can be used by ROS 2 nodes for perception and planning tasks.
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_gazebo/
mkdir config
Update CMakeLists.txt with the new config folder.
# Copy necessary files to designated locations in the project
install (
DIRECTORY config
DESTINATION share/${PROJECT_NAME}
)
Create a file called ros_gz_bridge.yaml inside the config folder.
Add this code.
# RGBD Camera topics
- ros_topic_name: "cam_1/color/camera_info"
gz_topic_name: "cam_1/camera_info"
ros_type_name: "sensor_msgs/msg/CameraInfo"
gz_type_name: "gz.msgs.CameraInfo"
direction: GZ_TO_ROS
lazy: true # Determines whether connections are created immediately at startup (when false) or only when data is actually requested by a subscriber (when true), helping to conserve system resources at the cost of potential initial delays in data flow.
- ros_topic_name: "cam_1/depth/color/points"
gz_topic_name: "cam_1/points"
ros_type_name: "sensor_msgs/msg/PointCloud2"
gz_type_name: "gz.msgs.PointCloudPacked"
direction: GZ_TO_ROS
lazy: true
# LIDAR configuration
- ros_topic_name: "scan"
gz_topic_name: "scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: GZ_TO_ROS
lazy: false
# IMU configuration
- ros_topic_name: "imu/data"
gz_topic_name: "imu/data"
ros_type_name: "sensor_msgs/msg/Imu"
gz_type_name: "gz.msgs.IMU"
direction: GZ_TO_ROS
lazy: false
# Sending velocity commands from ROS 2 to Gazebo
- ros_topic_name: "mecanum_drive_controller/cmd_vel"
gz_topic_name: "mecanum_drive_controller/cmd_vel"
ros_type_name: "geometry_msgs/msg/TwistStamped"
gz_type_name: "gz.msgs.Twist"
direction: ROS_TO_GZ
lazy: false
Save the file, and close it.
Create an RViz configuration file.
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_gazebo/
mkdir rviz
Add this file to the rviz folder: yahboom_rosmaster_gazebo_sim.rviz.
Update CMakeLists.txt
Go to the CMakeLists.txt.
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_gazebo/
Make sure it has this block of code:
# Copy necessary files to designated locations in the project
install (
DIRECTORY launch models worlds rviz
DESTINATION share/${PROJECT_NAME}
)
gedit CMakeLists.txt
Save the file, and close it.
yahboom_rosmaster.gazebo.launch.py
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_gazebo/launch
Create a new file called yahboom_rosmaster.gazebo.launch.py.
Let’s see the information coming across some of the ROS 2 topics.
ros2 topic echo /mecanum_drive_controller/cmd_vel displays the velocity commands sent to the robot, telling it how fast and in which direction to move.
ros2 topic echo /joint_states shows the current positions (in radians) and velocities (in radians per second) of the robot’s wheels.
ros2 topic echo/mecanum_drive_controller/odom provides information about the robot’s estimated position, orientation, linear, and angular velocity based on wheel rotation data.
ros2 topic echo /imu/data outputs readings from the robot’s inertial measurement unit (IMU), which includes data about angular velocity and linear acceleration.
ros2 topic echo /scan displays data from the robot’s laser scanner or LiDAR, providing information about the distances to obstacles in the environment.
You can see the /scan data, if you go to RViz, and click the Add button in the Displays panel on the left.
Click the “By topic” tab, and scroll down to LaserScan. Click on that, and click OK to add it.
You can see the LIDAR scan data.
Here is what things look like when launching the robot in pick_and_place_demo.world. You can see how the scans match with the objects in the environment.
To see the frequency of publishing, type:
ros2 topic hz /scan
To get information about this topic, type:
ros2 topic info /scan
I also recommend checking out /cam_1/color/image_raw in RViz to see if the camera transformation for the cam_1_depth_optical_frame is oriented correctly. Compare Gazebo (on the left) to what the camera is actually seeing in ROS 2 (on the right).
Click the “By topic” tab.
Click /cam_1 -> /depth -> /color -> /points -> PointCloud2 so you can see the point cloud in RViz.
While the point cloud data shows perfectly fine in my robotic arm applications with Gazebo and RViz, I had big time rendering issues when trying to get it to display in RViz. You can see these issues in the image below. I suspect it is related to the amount of memory on my virtual machine given all the sensor data that is flowing through from Gazebo to ROS 2.
Let’s create a ROS 2 node that can make the robot move repeatedly in a square pattern.
Let’s start by creating a program.
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_system_tests/src
Add a file called square_mecanum_controller.cpp.
/**
* @file square_mecanum_controller.cpp
* @brief Controls a mecanum wheeled robot to move in a square pattern
*
* This program creates a ROS 2 node that publishes velocity commands to make a
* mecanum wheeled robot move in a square pattern. It takes advantage of the
* omnidirectional capabilities of mecanum wheels to move in straight lines
* along both X and Y axes.
*
* Publishing Topics:
* /mecanum_drive_controller/cmd_vel (geometry_msgs/TwistStamped):
* Velocity commands for the robot's motion
*
* @author Addison Sears-Collins
* @date November 22, 2024
*/
#include <chrono>
#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
using namespace std::chrono_literals;
class SquareController : public rclcpp::Node
{
public:
SquareController() : Node("square_controller")
{
// Create publisher for velocity commands
publisher_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(
"/mecanum_drive_controller/cmd_vel", 10);
// Create timer that calls our control function every 200ms
timer_ = this->create_wall_timer(
200ms, std::bind(&SquareController::timer_callback, this));
// Initialize variables
current_side_ = 0; // Which side of square we're on (0-3)
elapsed_time_ = 0.0; // Time spent on current side
robot_speed_ = 0.2; // Speed in meters/second
side_length_ = 2.0; // Length of each side in meters
time_per_side_ = side_length_ / robot_speed_; // Time to complete one side
}
// Send stop command when node is shut down
~SquareController()
{
stop_robot();
}
// Function to stop the robot
void stop_robot()
{
auto msg = geometry_msgs::msg::TwistStamped();
msg.header.stamp = this->now();
publisher_->publish(msg); // All velocities default to 0
}
private:
void timer_callback()
{
// Create velocity command message
auto msg = geometry_msgs::msg::TwistStamped();
msg.header.stamp = this->now();
// Set velocity based on which side of the square we're on
switch (current_side_) {
case 0: // Moving forward (positive X)
msg.twist.linear.x = robot_speed_;
break;
case 1: // Moving left (positive Y)
msg.twist.linear.y = robot_speed_;
break;
case 2: // Moving backward (negative X)
msg.twist.linear.x = -robot_speed_;
break;
case 3: // Moving right (negative Y)
msg.twist.linear.y = -robot_speed_;
break;
}
// Publish the velocity command
publisher_->publish(msg);
// Update time tracking
elapsed_time_ += 0.2; // 200ms in seconds
// Check if we've completed the current side
if (elapsed_time_ >= time_per_side_) {
elapsed_time_ = 0.0;
current_side_ = (current_side_ + 1) % 4; // Move to next side (0-3)
}
}
// Class member variables
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr publisher_;
int current_side_;
double elapsed_time_;
double side_length_;
double robot_speed_;
double time_per_side_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<SquareController>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
In this tutorial, I will guide you through the process of creating a custom launch file to launch a robotic arm and a mobile robot 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.
Launch files in ROS 2 are powerful tools that allow you to start multiple nodes and set parameters with a single command, simplifying the process of managing your robot’s complex systems.
You don’t need to understand all the nitty gritty details of this configuration file. You can generate one automatically through RViz.
On a high-level, RViz configuration files end with the extension .rviz. These files set up an RViz configuration with a grid, a robot model, and coordinate frame visualizations.
This configuration file also enables the camera movement tool and sets the initial camera view to an orbit view, which allows orbiting around a focal point in the scene.
When RViz is launched with this configuration file, it will display the robot model and allow interaction and visualization of the robot.
Edit CMakeLists.txt
Now we need to edit CMakeLists.txt so the build system can find our new folders, launch and rviz.
cd ~/ros2_ws/src/mycobot_ros2/mycobot_description/
gedit CMakeLists.txt
Add this code:
# Copy necessary files to designated locations in the project
install (
DIRECTORY launch meshes urdf rviz
DESTINATION share/${PROJECT_NAME}
)
cd ~/ros2_ws/src/yahboom_rosmaster/yahboom_rosmaster_description/
gedit CMakeLists.txt
Add this code:
# Copy necessary files to designated locations in the project
install (
DIRECTORY launch meshes urdf rviz
DESTINATION share/${PROJECT_NAME}
)
And there you have it…your first custom launch files.
Launch File Walkthrough
Let’s walk through this ROS 2 launch file that visualizes a robotic arm in RViz.
Starting with the imports, we bring in essential ROS 2 launch utilities.
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
The launch file’s core purpose is orchestrating multiple nodes – think of it as a conductor coordinating different musicians in an orchestra. Here, our musicians are:
Robot State Publisher: Broadcasts the robot’s current pose
Joint State Publisher: Manages the robot’s joint positions
RViz: Provides the 3D visualization
The ARGUMENTS list defines the robot’s configurable parameters:
ARGUMENTS = [
DeclareLaunchArgument('prefix', default_value='',
description='Prefix for robot joints and links'),
DeclareLaunchArgument('add_world', default_value='true',
description='Whether to add world link'),
# ... other arguments
]
These arguments allow users to customize the robot’s configuration without changing the code – like using command-line switches to modify a program’s behavior.
The generate_launch_description() function is where the magic happens. First, it sets up the file paths:
