Coordinate Frame Basics and the Right-Hand Rule of Robotics

Welcome to this tutorial on three-dimensional coordinate frames for robots. Understanding 3D coordinate frames is essential for robots to determine their position and navigate the world effectively. Whether it’s picking up an item, avoiding obstacles, or moving around a room, robots rely on these frames to plan their movements with precision.

The Coordinate Axes

A 3D coordinate frame consists of three perpendicular axes that intersect at a common point called the origin. Each axis is typically represented by a different color for easy identification:

X-axis (red): Points forward.
Y-axis (green): Points to the left.
Z-axis (blue): Points upward.

Think of these axes as directions in space that help describe any position or movement.

The Right-Hand Rule

To remember the orientation of the coordinate axes, use the right-hand rule:

Hold out your right hand with your thumb, index finger, and middle finger all perpendicular to each other.
Assign your fingers to the axes:
- Index Finger: Points along the positive x-axis (forward).
- Middle Finger: Points along the positive y-axis (left).
- Thumb: Points along the positive z-axis (upward).

Understanding Rotation: Roll, Pitch, and Yaw

The right-hand rule helps us understand the basic orientation of our coordinate frame, but robots need to do more than just move along straight lines. They also need to rotate and change their orientation in space. This brings us to three fundamental types of rotation: roll, pitch, and yaw. Remember those terms…roll, pitch, and yaw.

Let’s relate these terms to head movements:

Roll (rotation around the x-axis) is like tilting your head from side to side, as if you’re touching your ear to your shoulder.
Pitch (rotation around the y-axis) is like nodding your head up and down.
Yaw (rotation around the z-axis) is like shaking your head ‘no’.

Now let’s relate these to the right-hand rule:

Rotate your hand as if turning a doorknob. That is roll.
Rotate your hand up and down, as if nodding your head “yes.” That is pitch
Rotate your hand left and right, as if shaking your head “no.”. That is yaw.

Coordinate Frame Hierarchy

Now that we understand how individual coordinate frames work and how objects can rotate within them, let’s explore how robots use multiple coordinate frames together. This system of related frames, known as a coordinate frame hierarchy, is important for robots to understand their place in the world and how their parts relate to each other.

World Coordinate Frame

The world coordinate frame, which can often be referred to as the map frame, serves as the fixed, global reference point for all robots and objects in a given environment. It never moves or changes, providing a stable point of reference. This frame is often placed at a convenient location, such as the center of a room’s floor or the battery charging station.

Think of the world frame as the “ground truth” of the environment. All other coordinate frames are ultimately referenced back to this frame, allowing different robots and sensors to understand each other’s positions and coordinate actions.

Robot-Specific Frames

Base Frame

The base frame is attached to the robot’s base or body and moves with the robot as it navigates.

For mobile robots, the base frame changes position relative to the world frame as the robot moves around.

For robotic arms, the base frame is typically fixed at the bottom/mount point of the arm. This fixed base frame serves as the primary reference point for all joint movements and gripper positions.

Sensor Frames

Think of sensor frames like the eyes and ears of the robot. Each camera, distance sensor, or touch sensor has its own frame (i.e. x, y, and z axis) that tells the robot what that sensor can “see” or “feel” from its specific location on the robot.

Joint Frames

For robots with arms or moving parts, each joint (like your elbow or wrist) has its own frame. These frames help the robot know how each joint is bent or twisted.

End-Effector Frame

This is like the robot’s “hand” – it’s the frame at the very end of a robotic arm where tools or grippers are attached. When a robot needs to pick something up or use a tool, it uses this frame to know exactly where its “hand” is.

Frame Relationships

Understanding the relationships between different frames is key to controlling a robot’s movements and interpreting its sensor data.

For example, imagine you want a robotic arm to pick up a ball on a table. The arm’s movements are defined in its local frame, but the ball’s position is given in the world (map) frame. By transforming the ball’s world coordinates into the arm’s frame, the robot can accurately reach and grasp it.

Practical Example

Consider a self-driving car:

The car’s starting position is the origin of its coordinate frame.
Moving forward means it’s traveling in the positive x direction.
Turning left or right involves rotation around the z-axis, which is its yaw movement.
If the car moves sideways, that’s along the y-axis.
If the car could jump (imagine it could), that would be along the z-axis.

Make Sure You Understand Coordinate Frames

Coordinate frames form the foundation of a robot’s spatial understanding. By maintaining clear relationships between different frames—such as the world (map) frame, robot base frame, sensor frames, and manipulator frames—robots can effectively plan and execute complex tasks.

By understanding 3D coordinate frames, you’ll be better equipped to program and control robots, whether you’re working on simple projects or advanced robotic systems.

To learn more about common coordinate frames specific to ROS 2 mobile robots, check out this tutorial.

That’s it. Keep building!

How to Create Unit Tests with GTest – ROS 2 Jazzy

This tutorial introduces unit testing in ROS 2 using GTest (GoogleTest), Google’s C++ testing framework. We’ll explore how to write and run tests for ROS 2 nodes, ensuring your robotic software functions correctly at the component level.

By the end, you’ll be equipped to implement comprehensive unit tests, enhancing the reliability and maintainability of your ROS 2 projects.

Prerequisites

You have completed this tutorial: How to Create a Subscriber Node in C++ – Jazzy (required).
You have completed this tutorial: How to Create Unit Tests with Pytest – ROS 2 Jazzy (required).
I am assuming you are using Visual Studio Code, but you can use any code editor.

You can find all the code here on GitHub.

Why Use GTest

Imagine you’re building a complex robot using ROS 2, with multiple C++ nodes working together to control its behavior. Every time you update your code, install a new version of ROS 2, or upgrade any dependencies, something could break without you realizing it. This is where GTest comes in handy.

GTest is like a safety net for your robot’s C++ code. It automatically checks that everything still works correctly after you make changes. For example, if you update your navigation code or install a new version of a sensor driver, GTest can quickly verify that your nodes are still publishing the right messages at the right times. This helps you catch problems early – before your robot starts behaving unexpectedly in the real world.

Create the Unit Test

Let’s write a basic unit test for our ROS 2 C++ publisher node. The test code we will write sets up a testing environment for a ROS 2 publisher node, making sure it can properly start up and shut down.

The first test checks if the node is created with the right name and is publishing on the correct topic. The second test verifies that the node actually publishes messages with the correct “Hello World!” format we expect.

Open a terminal window and move to your workspace:

cd ~/ros2_ws/

colcon build && source ~/.bashrc

cd ~/ros2_ws/src/ros2_fundamentals_examples/

Create a new folder:

mkdir -p test/gtest

Create the test file test/gtest/test_publisher.cpp:

/**
 * @file test_publisher.cpp
 * @brief Unit tests for the ROS 2 minimal publisher node.
 *
 * These tests verify the functionality of a minimal ROS 2 publisher including
 * node creation, message formatting, and publishing behavior.
 *
 * @author Addison Sears-Collins
 * @date November 6, 2024
 */

#include <gtest/gtest.h>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "../../src/cpp_minimal_publisher.cpp"

/**
 * @class TestMinimalPublisher
 * @brief Test fixture for the MinimalCppPublisher tests
 *
 * This fixture handles the setup and teardown of ROS 2 infrastructure
 * needed for each test case.
 */
class TestMinimalPublisher : public ::testing::Test 
{
protected:
    void SetUp() override 
    {
        rclcpp::init(0, nullptr);
        node = std::make_shared<MinimalCppPublisher>();
    }

    void TearDown() override 
    {
        node.reset();
        rclcpp::shutdown();
    }

    std::shared_ptr<MinimalCppPublisher> node;
};

/**
 * @test TestNodeCreation
 * @brief Verify the publisher node is created correctly
 *
 * Tests:
 * - Node name is set correctly
 * - Publisher exists on the correct topic
 * - Message type is correct
 */
TEST_F(TestMinimalPublisher, TestNodeCreation) 
{
    // Verify the node name is set correctly
    EXPECT_EQ(node->get_name(), "minimal_cpp_publisher");

    // Get all publisher endpoints
    auto pub_endpoints = node->get_publishers_info_by_topic("/cpp_example_topic");
    
    // Verify we have exactly one publisher on the topic
    EXPECT_EQ(pub_endpoints.size(), 1u);
    
    // Verify the topic name and message type
    EXPECT_EQ(pub_endpoints[0].topic_name, "/cpp_example_topic");
    EXPECT_EQ(pub_endpoints[0].topic_type, "std_msgs/msg/String");
}

/**
 * @test TestMessageContent
 * @brief Verify the published message content is correct
 *
 * Creates a subscription to capture published messages and verifies
 * the message format matches expectations.
 */
TEST_F(TestMinimalPublisher, TestMessageContent) 
{
    // Create a subscription to capture the published message
    std::shared_ptr<std_msgs::msg::String> received_msg;
    auto subscription = node->create_subscription<std_msgs::msg::String>(
        "/cpp_example_topic", 10,
        [&received_msg](const std_msgs::msg::String::SharedPtr msg) {
            received_msg = std::make_shared<std_msgs::msg::String>(*msg);
        });

    // Trigger the timer callback
    node->timerCallback();

    // Spin until we receive the message or timeout
    rclcpp::spin_some(node);
    
    // Verify the message format
    ASSERT_NE(received_msg, nullptr);
    EXPECT_EQ(received_msg->data, "Hello World! 0");
}

/**
 * @brief Main function to run all tests
 */
int main(int argc, char** argv) 
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

Edit the C++ Publisher

Open cpp_minimal_publisher.cpp.

We need to wrap the main function in the TESTING_EXCLUDE_MAIN condition so it won’t be included during testing. Here’s how to modify it:

#ifndef TESTING_EXCLUDE_MAIN
int main(int argc, char * argv[])
{
  // Initialize ROS 2.
  rclcpp::init(argc, argv);

  // Create an instance of the MinimalCppPublisher node and keep it running.
  auto minimal_cpp_publisher_node = std::make_shared<MinimalCppPublisher>();
  rclcpp::spin(minimal_cpp_publisher_node);

  // Shutdown ROS 2 upon node termination.
  rclcpp::shutdown();

  // End of program.
  return 0;
}
#endif

By adding the #ifndef TESTING_EXCLUDE_MAIN and #endif, this main function will only be compiled when we’re not running tests. When we include this file in our test file (where we defined TESTING_EXCLUDE_MAIN), this main function will be skipped.

Edit CMakeLists.txt

Update CMakeLists.txt. Find the if(BUILD_TESTING) block and replace it with:

if(BUILD_TESTING)
  # Find required test packages
  find_package(ament_lint_auto REQUIRED)
  find_package(ament_cmake_pytest REQUIRED)
  find_package(ament_cmake_gtest REQUIRED)
  
  # Python tests
  ament_add_pytest_test(minimal_publisher_test test/pytest/test_publisher.py
    TIMEOUT 60
  )
  
  # C++ tests
  ament_add_gtest(cpp_minimal_publisher_test
    test/gtest/test_publisher.cpp
  )
  
  # Add dependencies for the C++ test executable
  ament_target_dependencies(cpp_minimal_publisher_test
    ${dependencies}
  )
  
  # Optional: Enable other linting tools
  #set(ament_cmake_copyright_FOUND TRUE)
  #set(ament_cmake_cpplint_FOUND TRUE)
  #ament_lint_auto_find_test_dependencies()
endif()

Edit package.xml

We need to add this to the package.xml so that our tests are discoverable to cmake:

<test_depend>ament_cmake_gtest</test_depend>

Save the file, and close it.

Build and Run

Build and run the tests:

cd ~/ros2_ws

colcon build --packages-select ros2_fundamentals_examples

source ~/.bashrc

colcon test --packages-select ros2_fundamentals_examples

To see detailed test results, type:

colcon test --packages-select ros2_fundamentals_examples --event-handlers console_direct+

When we run the tests using colcon test, it first runs the Python tests which all pass (“3 passed in 0.37s”). Then it runs our two C++ GTest tests: one checking the node name and publisher setup, and another checking the message content. Both C++ tests pass as shown by the output “[ PASSED ] 2 tests” and “100% tests passed, 0 tests failed out of 2”, indicating that our publisher node is working exactly as intended.

To check test results:

colcon test-result --all

To see which test cases failed (if any):

colcon test-result --all --verbose

That’s it. Keep building!

How to Create Unit Tests with Pytest – ROS 2 Jazzy

Imagine you’re building a complex robot using ROS 2, the latest version of the Robot Operating System. Your robot is made up of many different parts, each controlled by a piece of software. Now, how do you make sure all these pieces work correctly? That’s where unit testing comes in.

Unit testing is like doing a health check-up for each part of your robot’s software. It’s a way to automatically test small pieces of your code to make sure they’re doing exactly what they’re supposed to do.

Unit tests are essential tools in robotics software development for two key reasons:

Early Bug Detection: Automate the testing of code to ensure that changes or updates don’t introduce bugs.
Improved Code Design: Writing testable code leads to cleaner, more modular software.

In this tutorial, I’ll show you how to use a tool called Pytest to write and run these unit tests. With Pytest, you can write tests for individual parts of your code, such as functions and classes, to make sure each part behaves as expected.

Pytest is designed primarily for Python code, so it’s commonly used for testing Python-based ROS 2 packages. For C++ code in ROS 2, other testing frameworks, like GTest, are typically used instead.

Prerequisites

You have completed this tutorial: How to Create a ROS 2 Python Publisher – Jazzy (required)
You have completed this tutorial: How to Create a Subscriber Node in C++ – Jazzy (recommended)
I am assuming you are using Visual Studio Code, but you can use any code editor.

You can find all the code here on GitHub.

Create a Unit Test

Let’s write a basic unit test for our ROS 2 publisher node. This test will verify three key things:

that our node is created correctly
that its message counter works properly
that it formats messages as expected.

We’ll create a Python test file that uses Pytest to run these checks automatically.

Open a terminal window, and move to your workspace.

cd ~/ros2_ws/

colcon build && source ~/.bashrc

cd ~/ros2_ws/src/ros2_fundamentals_examples/

mkdir -p test/pytest

Create a test file test/pytest/test_publisher.py:

#!/usr/bin/env python3
"""
Test suite for the ROS2 minimal publisher node.

This script contains unit tests for verifying the functionality of a minimal ROS2 publisher.
It tests the node creation, message counter increment, and message content formatting.

Subscription Topics:
    None

Publishing Topics:
    /py_example_topic (std_msgs/String): Example messages with incrementing counter

:author: Addison Sears-Collins
:date: November 6, 2024
"""

import pytest
import rclpy
from std_msgs.msg import String
from ros2_fundamentals_examples.py_minimal_publisher import MinimalPyPublisher


def test_publisher_creation():
    """
    Test if the publisher node is created correctly.

    This test verifies:
    1. The node name is set correctly
    2. The publisher object exists
    3. The topic name is correct

    :raises: AssertionError if any of the checks fail
    """
    # Initialize ROS2 communication
    rclpy.init()
    try:
        # Create an instance of our publisher node
        node = MinimalPyPublisher()

        # Test 1: Verify the node has the expected name
        assert node.get_name() == 'minimal_py_publisher'

        # Test 2: Verify the publisher exists and has the correct topic name
        assert hasattr(node, 'publisher_1')
        assert node.publisher_1.topic_name == '/py_example_topic'
    finally:
        # Clean up ROS2 communication
        rclpy.shutdown()


def test_message_counter():
    """
    Test if the message counter increments correctly.

    This test verifies that the counter (node.i) increases by 1 after
    each timer callback execution.

    :raises: AssertionError if the counter doesn't increment properly
    """
    rclpy.init()
    try:
        node = MinimalPyPublisher()
        initial_count = node.i
        node.timer_callback()
        assert node.i == initial_count + 1
    finally:
        rclpy.shutdown()


def test_message_content():
    """
    Test if the message content is formatted correctly.

    This test verifies that the message string is properly formatted
    using an f-string with the current counter value.

    :raises: AssertionError if the message format doesn't match expected output
    """
    rclpy.init()
    try:
        node = MinimalPyPublisher()
        # Set counter to a known value for testing
        node.i = 5
        msg = String()
        # Using f-string instead of % formatting
        msg.data = f'Hello World: {node.i}'
        assert msg.data == 'Hello World: 5'
    finally:
        rclpy.shutdown()


if __name__ == '__main__':
    pytest.main(['-v'])

Edit CMakeLists.txt

Now open CMakeLists.txt, and replace the if(BUILD_TESTING) block with this code:

if(BUILD_TESTING)
  #find_package(ament_lint_auto REQUIRED)
  find_package(ament_cmake_pytest REQUIRED)
  #set(ament_cmake_copyright_FOUND TRUE)
  #set(ament_cmake_cpplint_FOUND TRUE)
  #ament_lint_auto_find_test_dependencies()

  ament_add_pytest_test(minimal_publisher_test test/pytest/test_publisher.py
   TIMEOUT 60
  )
endif()

Edit the package.xml File

We need to add this to the package.xml so that our tests are discoverable to cmake:

<test_depend>ament_cmake_pytest</test_depend>

Save the file, and close it.

Build and Run

Compile and run the tests.

cd ~/ros2_ws

rosdep install --from-paths src --ignore-src -r -y

colcon build --packages-select ros2_fundamentals_examples

source ~/.bashrc

colcon test --packages-select ros2_fundamentals_examples

To get more detail, you could also have done:

colcon test --packages-select ros2_fundamentals_examples --event-handlers console_direct+

When we run our unit tests with this command above (Starting >>> ros2_fundamentals_examples), it first starts by loading our ROS 2 package.

The system then collects and runs our three test cases, shown as the three passing tests in the output (test/pytest/test_publisher.py …). These dots represent successful runs of our three tests – publisher node creation, message counter, and message content.

Your test results should show all tests passed successfully (============================== 3 passed in 0.28s ===============================).

My final summary shows that our single test suite (1/1 Test #1: minimal_publisher_test) passed successfully in 0.76 seconds, with CMake reporting 100% success rate as no tests failed.

Now once that is finished, check the results:

colcon test-result --all

To see which test cases failed, type:

colcon test-result --all --verbose

That’s it.

Keep building!