Simulation Test

diffbot_gazebo has a simulation test: it drives DiffBot and Remo in Gazebo Classic 11, the simulator that ROS Noetic uses, and checks where they went. Both CI workflows run it on every pull request (diffbot#110). It catches mistakes in the robot model and the controller configuration, which the simulation shares with the real robot:

• Left and right wheel joints swapped: the robot turns right when it's told to turn left.
• A wheel radius in the controller configuration that doesn't match the robot model: the odometry can still report about the expected distance while the robot drives a different one.
• A wrong joint name: the controller doesn't start, and nothing is published.

You need the dev container with its workspace ~/catkin_ws built and sourced, as the container's setup leaves it. This page first shows how to run the test and read its results, then how it works and its code step by step, then why the results differ from the commands, and finally how to add a test of your own.

Run it

In the dev container, in ~/catkin_ws, run all tests of the workspace as CI does (dev container workflow):

catkin build --catkin-make-args run_tests
catkin_test_results build

The first command builds the workspace and runs every registered test, but it doesn't fail when a test fails. The second one sums up the result files and fails if any test failed.

To run the test for one robot, use its test file, sim_drive_diffbot.test or sim_drive_remo.test:

rostest diffbot_gazebo sim_drive_diffbot.test

With --text, rostest prints the nodes' output in the terminal instead of log files, but it doesn't collect the test results then: it can report success although a check failed. Use --text to watch a run, and the command without it to check the result.

Read the results

rostest ends with a summary. A passing run of sim_drive_diffbot.test:

[diffbot_gazebo.rosunit-drive_diffbot/test_forward][passed]
[diffbot_gazebo.rosunit-drive_diffbot/test_turn][passed]
[diffbot_gazebo.rosunit-publish_diffbot/test_publish][passed]

SUMMARY
 * RESULT: SUCCESS
 * TESTS: 3
 * ERRORS: 0
 * FAILURES: 0

The test file runs two test nodes (step 1): drive_diffbot, which has the two test methods test_forward and test_turn, and publish_diffbot, which has one test. A failed check shows [FAILURE] instead of [passed], with the check's message, which names the measured value, the expected one and the tolerance, followed by both measured motions; the result is then FAIL. An error means that a test couldn't get to its checks, for example because no odometry arrived.

catkin_test_results build counts the tests in all result files. For this test, it reports 10: for each robot, rostest's own result file lists the two test nodes, and the nodes' files list their two drive tests and the one publish test.

The measured motions: measure_motion() logs both measurements of each drive, and rostest copies the node's log into its result file rosunit-drive_diffbot.xml. The file is in ~/.ros/test_results/diffbot_gazebo/ when you run rostest yourself, and in build/diffbot_gazebo/test_results/diffbot_gazebo/ when catkin runs the tests:

grep -o '\(odometry\|true pose\): [^<&]*' ~/.ros/test_results/diffbot_gazebo/rosunit-drive_diffbot.xml

odometry: forward 0.580 m, left 0.001 m, turned 0.005 rad
true pose: forward 0.590 m, left 0.001 m, turned 0.004 rad
odometry: forward 0.006 m, left 0.002 m, turned 1.370 rad
true pose: forward 0.019 m, left -0.016 m, turned 1.263 rad

Each drive is measured twice: with the robot's odometry and with its true pose in Gazebo (How it works). The first two lines are test_forward, which commands 0.6 m straight ahead; the last two are test_turn, which commands 1.5 rad on the spot. forward, left and turned describe the motion as seen from where the robot started (relative_pose()). The numbers aren't exactly the commanded ones: the checks accept a range around each expected value (the checks), and Why the results differ from the commands explains where the differences come from.

How it works

What runs in the simulation

The test file starts Gazebo, the robot and its controllers, and then the test nodes. The first tab of the figure shows them during the test; the second tab shows the same controllers on the real robot (On the real robot).

Simulation testReal robot

Topic names are shortened in the figure: cmd_vel and odom are under /diffbot/mobile_base_controller/, joint_states, wheel_cmd_velocities and measured_joint_states under /diffbot/, and model_states under /gazebo/. The figure shows both test nodes, although rostest runs them one after another (step 1).

The robot's controllers come from ros_control. ros_control isn't a node of its own: it's a set of C++ libraries, used inside the process that has access to the joints, here the Gazebo server. The robot's URDF (Unified Robot Description Format), the XML file that describes its links and joints, loads the gazebo_ros_control plugin (robot.gazebo.xacro). The plugin creates the hardware interface and the controller manager, and the controller manager loads the controllers:

• The hardware interface, DefaultRobotHWSim, gives the controllers each wheel joint's position and velocity, and takes a velocity command for each wheel. It reads the values from the simulated joints, for the wheel joints that have a transmission in the URDF, and drives each joint to its commanded velocity with a PID controller, with the gains from pid.yaml.
• The controller manager updates the controllers. In each simulation step, the plugin reads the joints (readSim()), lets the controller manager update the controllers, and applies the commands (writeSim()).
• The controllers read and write the hardware interface's values directly, in the same process, not over topics. joint_state_controller copies the joint values to joint_states. mobile_base_controller, a diff_drive_controller, turns the velocities from cmd_vel into a velocity for each wheel, and computes the odometry from the wheel joint positions. Robot Motion shows how: with the velocity motion model of Probabilistic Robotics.

Three views of the motion

The test compares what the robot should do with what it did, seen in two ways:

View	Topic	Published by	What it is
The command	/diffbot/mobile_base_controller/cmd_vel	test_drive.py	A linear and an angular velocity. Sent for 3 s, they give the expected motion, for example 0.2 m/s for 3 s is 0.6 m
The odometry	/diffbot/mobile_base_controller/odom	diff_drive_controller	The pose that the controller computes from the wheel angles, with the wheel radius and separation from diffbot_control.yaml
The true pose	/gazebo/model_states	Gazebo's ROS interface, not ros_control	The pose of every model in Gazebo's world, from the physics simulation

Three comparisons. The test compares each measurement with the expected motion, and the two measurements with each other:

Comparison	Question it answers	Example failure	Tolerance
Odometry and expected motion	Does the motion that the controller computes from the wheels follow the command?	The odometry reports no motion, or the wrong direction or distance	Wide: it allows for how the drive responds to the command
True pose and expected motion	Does the robot really drive it?	It turns the wrong way, drives off to the side, or drives a clearly wrong distance	Wide: it allows for how the drive responds to the command
Odometry and true pose	Do the reported and the real motion agree?	They differ in distance or turn, for example because of the wheel geometry or slip	Narrower: based on how far apart they are in normal runs

The comparisons overlap, and a failed one doesn't name the cause. The agreement checks compare the distance in test_forward() and the turn in test_turn(). If no odometry arrives at all, for example because the controller didn't start, the test fails before it drives: setUpClass() waits for the first message (Connecting to the robot).

Why the odometry alone can miss configuration errors. The controller uses its configuration twice: on the way out, to turn the command into wheel speeds, and on the way back, to turn the wheel angles into the odometry. A wrong wheel radius can cancel out:

The configured radius \(r_\text{cfg}\) from diffbot_control.yaml is used twice on the way to the odometry, the model's radius \(r_\text{model}\) from the URDF once on the way to the true pose.

For a straight drive at \(v\) for the time \(t\), with the configured radius \(r_\text{cfg}\) and the model's radius \(r_\text{model}\), and assuming that the wheels follow their speed command exactly and don't slip, the wheels turn by the angle \(\Delta\phi\) (rad), and:

\[ \begin{aligned} \Delta\phi &= \frac{v\,t}{r_\text{cfg}} \\ s_\text{odom} &= r_\text{cfg}\,\Delta\phi = v\,t \\ s_\text{true} &= r_\text{model}\,\Delta\phi = v\,t\,\frac{r_\text{model}}{r_\text{cfg}} \end{aligned} \]

A configured radius that's too big makes the controller command fewer wheel turns. Multiplied by the same too-big radius, they still give the expected distance in the odometry, while the model's smaller wheels roll less far. So the odometry still matches the expected motion, and only the two comparisons that involve the true pose can show the mistake: the true pose against the expected motion, and the odometry against the true pose. A wrong wheel separation cancels in the same way when the robot turns: the controller uses it to turn the commanded angular velocity into wheel speeds, and again to turn the wheels' distances into the odometry's angle. Swapped wheel joints cancel too: the controller sends the left wheel's command to the right wheel, so the robot turns right, but it also reads the right wheel's angle as the left one's, so its odometry still reports the commanded left turn.

/diffbot/joint_states, published by joint_state_controller, has each wheel's angle (rad) and velocity (rad/s). The test doesn't use it: a pose computed from it with the same wheel radius, separation and joint names would repeat the same mistakes.

Why the agreement checks can be narrower. Both measurements observe the same drive, so the timing and the wheel speed affect both. Their difference is usually smaller than either one's difference from the expected motion; the wheel geometry and slip still make them disagree (Why the odometry and the true pose differ).

What a pass shows. "True" means the pose of the robot model in Gazebo, not of the real robot. A passing test shows that the robot model and the controller configuration work together in the simulation; it doesn't calibrate the real robot. The tolerances allow what the robots measure in normal runs and reject the mistakes that were tried on purpose, but smaller mistakes may pass (the checks).

On the real robot

Switch the figure to the second tab, Real robot. The blue box stays the same: the controller manager with the two controllers, configured by diffbot_control.yaml. Only the hardware interface and what's below it change.

On the real robot, the diffbot_base node takes Gazebo's place. It creates the hardware interface DiffBotHWInterface and a controller manager, and runs the loop itself: read(), update the controllers, write(). Its launch file loads the same diffbot_control.yaml and spawns the same two controllers. This hardware interface talks to the Teensy over ROS topics:

• It subscribes to measured_joint_states, the wheel angles and velocities that the Teensy publishes, and read() passes the latest values to the controllers.
• write() publishes the wheel velocity commands on wheel_cmd_velocities, and the Teensy's firmware runs a PID loop per wheel to reach them (Base Hardware Interface).
• The rosserial node relays both topics over USB.

So the simulation test covers the robot model and the controller configuration that both share. It doesn't run diffbot_base, rosserial or the firmware; tests for these without the robot are planned in diffbot#114. ROS Control, An overview explains ros_control in more detail.

Code walkthrough

Two levels run the test. rostest runs the test file's test nodes one after another, each with its own launch (step 1). Inside the drive test node, unittest runs setUpClass() once and then the test methods (steps 3 to 5).

test_forward() calls measure_motion(), which calls drive(), stop() and, once for the odometry and once for the true pose, relative_pose(). Then test_forward() checks the two motions it got back. While this runs, the subscriber callbacks keep the latest odometry pose and true pose up to date in the background. test_turn() uses the same measure_motion().

The code blocks show the files at the diffbot commit that this page pins, with their comments, and the line numbers are the files'. The link above each block opens the same lines on GitHub.

1. The test file

A .test file is a launch file for rostest. sim_drive_diffbot.test:

<launch>
    <!-- Starts the diffbot model in Gazebo and runs two tests on it: publishtest checks that the
         controllers publish, test_drive.py drives the robot and checks its odometry and true pose -->
    <include file="$(find diffbot_gazebo)/test/sim_drive.launch">
        <arg name="model" value="diffbot"/>
        <arg name="gazebo_port" value="11351"/>
    </include>

    <!-- publishtest is a test node that comes with rostest (http://wiki.ros.org/rostest/Nodes#publishtest):
         it passes when each topic below gets a message within its timeout. negative: True would
         check the opposite, that nothing is published -->
    <test test-name="publish_diffbot" pkg="rostest" type="publishtest" time-limit="120">
        <rosparam>
            topics:
              - name: /diffbot/joint_states
                timeout: 90
                negative: False
              - name: /diffbot/mobile_base_controller/odom
                timeout: 90
                negative: False
        </rosparam>
    </test>

    <test test-name="drive_diffbot" pkg="diffbot_gazebo" type="test_drive.py" time-limit="180"/>
</launch>

rostest turns each <test> element into a test of its own and runs them one after another, in alphabetical order of their names: first drive_diffbot, then publish_diffbot. For each one, it starts a ROS master, launches everything in the file that isn't a test node, here sim_drive.launch (step 2), and runs the test node until it ends. Then it reads the node's results and stops everything again (runner.py). So each test node gets a freshly started Gazebo, and the two don't affect each other. time-limit is how long rostest waits for a node before it counts the test as failed.

sim_drive_remo.test differs only in the robot model, the port and the test names.

2. The launch file

sim_drive.launch starts the robot in Gazebo and its controllers:

<launch>
    <!-- For the simulation tests: the robot in Gazebo without a GUI, only the Gazebo server, which
         needs no display (as in CI), on the empty world, with its controllers from diffbot_control.
         Noetic's empty_world.launch ignores its headless argument, so it isn't set here -->
    <arg name="model" doc="model type [diffbot, remo]"/>
    <!-- catkin runs the test files in parallel, each with its own ROS master; each Gazebo server
         also needs its own port, and none uses Gazebo's default 11345, which a simulation that's
         open in the dev container may use -->
    <arg name="gazebo_port" doc="port of this test's Gazebo server"/>
    <env name="GAZEBO_MASTER_URI" value="http://localhost:$(arg gazebo_port)"/>

    <include file="$(find diffbot_gazebo)/launch/diffbot_view.launch">
        <arg name="model" value="$(arg model)"/>
        <arg name="gui" value="false"/>
        <arg name="world_name" value="$(find diffbot_gazebo)/worlds/diffbot_empty.world"/>
    </include>

    <include file="$(find diffbot_control)/launch/diffbot_control.launch">
        <arg name="model" value="$(arg model)"/>
        <arg name="rqt_robot_steering" value="false"/>
    </include>
</launch>

• diffbot_view.launch starts the Gazebo server on the empty world, which has a ground plane for the robot to stand on and a light. It runs with simulation time: Gazebo publishes its clock, and all nodes use it instead of the computer's clock (use_sim_time). On a slow computer the test takes longer, but the robot drives the same distance. Then spawn_model adds the robot from its URDF to the world, at x = −2 m, y = −0.5 m, while the simulation is already running.
• diffbot_control.launch loads diffbot_control.yaml, the controller configuration that the real robot uses too, and pid.yaml, the gains of Gazebo's wheel velocity controllers. Its controller_spawner asks the controller manager to load and start the two controllers. It also starts robot_state_publisher, which the test doesn't use. rqt_robot_steering, a window with sliders to drive the robot, stays off, because it needs a display.

How it works shows how these parts work together.

3. Connecting to the robot

rostest starts test_drive.py as a ROS node. Its last line, rostest.rosrun('diffbot_gazebo', 'test_drive', TestDrive), runs the unittest class TestDrive and writes the results to a file that rostest reads. unittest calls setUpClass() once, then the test methods. test_drive.py, lines 113–133:

class TestDrive(unittest.TestCase):
    """Drive tests for the robot that the rostest file starts in Gazebo."""

    # Set in setUpClass() and in the subscriber callbacks. They belong to the class, not to a
    # TestDrive object: unittest creates a new object for each test method, while setUpClass()
    # starts the node, its publisher and its subscribers only once
    cmd_vel: rospy.Publisher
    # The latest pose from the odometry and from Gazebo, kept up to date by on_odom() and
    # on_model_states()
    odom_pose: Pose2D
    true_pose: Pose2D

    @classmethod
    def setUpClass(cls) -> None:
        rospy.init_node('test_drive')
        cls.cmd_vel = rospy.Publisher(CMD_VEL_TOPIC, Twist, queue_size=1)
        rospy.Subscriber(ODOM_TOPIC, Odometry, cls.on_odom)
        rospy.Subscriber(MODEL_STATES_TOPIC, ModelStates, cls.on_model_states, queue_size=1)
        # Gazebo and the controllers take a while to start, especially in CI
        rospy.wait_for_message(ODOM_TOPIC, Odometry, timeout=120)
        rospy.wait_for_message(MODEL_STATES_TOPIC, ModelStates, timeout=30)

• rospy.init_node() registers the node with the ROS master. The constants at the top of the file, such as CMD_VEL_TOPIC, hold the full topic names.
• The publisher sends velocity commands, Twist messages, to the controller.
• The two subscribers call on_odom() for each odometry message, and on_model_states() for each model_states message. Gazebo publishes model_states in every simulation step, so queue_size=1 keeps only the newest message instead of falling behind.
• rospy.wait_for_message() waits for the first message on a topic. If none comes within the timeout, it raises an exception and the tests fail, for example when the controller doesn't start because of a wrong joint name.

The callbacks, lines 135–145:

@classmethod
def on_odom(cls, msg: Odometry) -> None:
    cls.odom_pose = to_pose2d(msg.pose.pose)

@classmethod
def on_model_states(cls, msg: ModelStates) -> None:
    # The message lists every model in the world, here the ground plane and the robot, with
    # the poses in the same order as the names. Gazebo publishes it before the robot is
    # spawned, so the first messages don't contain the robot
    if MODEL_NAME in msg.name:
        cls.true_pose = to_pose2d(msg.pose[msg.name.index(MODEL_NAME)])

rospy calls them in background threads whenever a message arrives, so odom_pose and true_pose always hold the latest pose. msg.name.index(MODEL_NAME) finds the robot's position in the list of model names, and its pose is at the same position in msg.pose.

Both convert the ROS pose to a Pose2D, a named tuple with \(x\), \(y\) and the heading \(\theta\), as on Robot Representations. Lines 69–78:

def to_pose2d(pose: Pose) -> Pose2D:
    """Convert a geometry_msgs/Pose to a Pose2D.

    ROS messages give the orientation as a quaternion. euler_from_quaternion converts it to roll,
    pitch and yaw, the rotations about the x, y and z axes. The robot drives on flat ground, so
    roll and pitch stay about 0, and the yaw is its heading theta.
    """
    q = pose.orientation
    _, _, yaw = euler_from_quaternion([q.x, q.y, q.z, q.w])
    return Pose2D(pose.position.x, pose.position.y, yaw)

A quaternion has four numbers that describe any rotation in 3D, without the special cases of three angles (tf2 quaternion tutorial). ROS's axes and angles follow REP 103: x forward, y to the left, z up, and the yaw grows when the robot turns to the left.

4. test_forward()

The constants at the top of the file name the commands, lines 36–44:

# The two tests' commands, each sent for DURATION seconds of simulation time
DURATION = 3.0  # s
FORWARD_VELOCITY = 0.2  # m/s
TURN_VELOCITY = 0.5  # rad/s
# How often drive() sends the command. The controller stops the robot when no command arrives
# for 0.5 s (diff_drive_controller's default cmd_vel_timeout), and drive() checks the end time
# once per cycle, so the robot drives nominally up to one cycle too long, here 1 cm or 0.025 rad.
# A late wake-up or message can add more; the tolerances leave room for it
COMMAND_RATE = 20  # Hz

test_forward(), with the comment before the two tests, lines 179–201:

# unittest runs the test methods in alphabetical order, here forward and then turn. Each one
# checks only its own motion, from wherever the robot is when it starts, so neither depends
# on the other. Each check compares a measured value with the expected one, within a
# tolerance that allows what the robots measure in Gazebo and rejects the mistakes that were
# tried on purpose. msg=motions adds both measured motions to a check's failure message,
# because CI shows the message but not the logged motions

def test_forward(self) -> None:
    odom_motion, true_motion = self.measure_motion(
        linear_velocity=FORWARD_VELOCITY, angular_velocity=0.0, duration=DURATION)
    motions = f'odometry {odom_motion}, true pose {true_motion}'
    # v * t = 0.6 m straight ahead. The controller's acceleration limit costs 3 cm when the
    # robot starts and gives them back when it brakes; 0.56 to 0.62 m measured. A robot that
    # doesn't move, or drives much too slow or too fast, fails
    expected_forward = FORWARD_VELOCITY * DURATION
    for motion in (odom_motion, true_motion):
        self.assertAlmostEqual(motion.forward, expected_forward, delta=0.1, msg=motions)
        # Straight ahead (about 1 mm to the side measured), and without turning
        self.assertAlmostEqual(motion.left, 0.0, delta=0.1, msg=motions)
        self.assertAlmostEqual(motion.turned, 0.0, delta=0.2, msg=motions)
    # The odometry and the true distance are 0.010 to 0.011 m apart. A wheel_radius in
    # diffbot_control.yaml 10 % bigger or smaller than now gave about 0.044 m and 0.076 m
    self.assertAlmostEqual(odom_motion.forward, true_motion.forward, delta=0.03, msg=motions)

It has two parts. measure_motion() drives the robot at 0.2 m/s for 3 s without turning, and returns how the robot moved, measured twice: odom_motion with the odometry, and true_motion with the true pose. Each one is a RelativePose, with how far the robot moved forward and to the left, and how much it turned (relative_pose()). motions describes both, for the checks' failure messages. The rest are the checks (The checks). The call names its arguments, so it reads without looking up measure_motion().

measure_motion()

Lines 164–177:

def measure_motion(self, linear_velocity: float, angular_velocity: float,
                   duration: float) -> Tuple[RelativePose, RelativePose]:
    """Drive, and return the motion measured with the odometry and with the true pose.

    Each motion is the end pose relative to the start pose.
    """
    odom_start, true_start = self.odom_pose, self.true_pose
    self.drive(linear_velocity, angular_velocity, duration)
    self.stop()
    odom_motion = relative_pose(odom_start, self.odom_pose)
    true_motion = relative_pose(true_start, self.true_pose)
    for source, motion in (('odometry', odom_motion), ('true pose', true_motion)):
        rospy.loginfo('%s: forward %.3f m, left %.3f m, turned %.3f rad', source, *motion)
    return odom_motion, true_motion

It keeps the latest odometry pose and true pose as the start poses, drives (drive()), stops (stop()), and computes, for each source, where the robot ended up relative to where it started (relative_pose()). The logged lines are the ones in Read the results.

drive()

Lines 147–157:

def drive(self, linear_velocity: float, angular_velocity: float, duration: float) -> None:
    """Send the velocity command (m/s, rad/s) for duration seconds of simulation time."""
    cmd = Twist()
    cmd.linear.x = linear_velocity
    cmd.angular.z = angular_velocity
    # Send the command repeatedly, so the controller doesn't stop the robot (see COMMAND_RATE)
    rate = rospy.Rate(COMMAND_RATE)
    end_time = rospy.Time.now() + rospy.Duration(duration)
    while rospy.Time.now() < end_time and not rospy.is_shutdown():
        self.cmd_vel.publish(cmd)
        rate.sleep()

• A Twist holds a linear and an angular velocity, each as a 3D vector. A differential drive robot can only drive along its x axis, linear.x in m/s, and turn about its z axis, angular.z in rad/s, positive to the left. The controller ignores the other values.
• rospy.Time.now() and rospy.Rate follow Gazebo's clock (step 2), so duration and COMMAND_RATE are in simulation time. Why the command is sent 20 times per second is in the comment above COMMAND_RATE.

stop()

Lines 159–162:

def stop(self) -> None:
    """Send a zero velocity and wait a second for the robot to come to rest."""
    self.cmd_vel.publish(Twist())
    rospy.sleep(1.0)

Twist() is a command with all velocities 0. The controller brakes as gently as it speeds up: from 0.2 m/s at 0.6 m/s², the robot needs 0.33 s to stop. One second of simulation time is enough for the robot to stand still, and for the odometry and Gazebo to publish its end pose.

relative_pose()

The odometry and Gazebo give poses in different frames, so the test compares the motion between two poses instead of the poses: the end pose relative to the start pose, in the robot's frame at the start.

The end pose relative to the start pose, in the robot's frame at the start (\(x_r, y_r\)): how far the robot moved forward and to the left, and the angle it turned.

With the displacement \(\Delta x = x_1 - x_0\) and \(\Delta y = y_1 - y_0\) in the frame of the poses, rotating it by \(-\theta_0\) gives it in the robot's frame at the start. A robot's x axis points forward and its y axis to the left (REP 103), which gives the two parts their names:

\[ \begin{bmatrix} \text{forward} \\ \text{left} \end{bmatrix} = \begin{bmatrix} \cos\theta_0 & \sin\theta_0 \\ -\sin\theta_0 & \cos\theta_0 \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} \]

and the angle it turned is \(\text{turned} = \theta_1 - \theta_0\).

The matrix is the transpose of the rotation matrix \(R(\theta_0)\), which turns a vector from the robot's frame into the frame of the poses; the transpose turns it back. This is the relative pose, a standard operation on poses: with homogeneous transforms, it is \(T_0^{-1} T_1\) (Peter Corke, Robotics, Vision and Control, chapter 2, Amazon.com, Amazon.de, affiliate links). Moving or rotating the frame of the poses changes both poses the same way, so the relative pose stays the same, and the motion from the odometry and from the true pose can be compared.

RelativePose holds the three numbers, lines 55–66:

class RelativePose(NamedTuple):
    """A pose relative to an earlier one, in the robot's frame at the earlier pose.

    They are the x, y and theta of the later pose in that frame, named after the directions of a
    robot's axes in ROS (REP 103): forward and left (m) are along the robot's x axis, which points
    forward, and its y axis, which points to the left. turned (rad) is the change of its heading,
    positive to the left.
    """

    forward: float
    left: float
    turned: float

relative_pose() computes them, with wrap() for the angle, lines 81–110:

def wrap(angle: float) -> float:
    """Return the same angle in -pi..pi.

    theta jumps from pi to -pi when the robot turns past 180 degrees, so a difference of two
    headings can be off by 2 pi. sin and cos don't change by 2 pi, and atan2 returns the angle
    of the point (cos, sin) in -pi..pi.
    """
    return math.atan2(math.sin(angle), math.cos(angle))


def relative_pose(start_pose: Pose2D, end_pose: Pose2D) -> RelativePose:
    """Return the end pose relative to the start pose, in the robot's frame at the start pose.

    The odometry and Gazebo give poses in different frames: the odometry starts at 0, 0, 0 where
    the controller started, while Gazebo's world frame has the robot spawned at x = -2, y = -0.5.
    The relative pose is the same in both frames, so the two can be compared. With homogeneous
    transforms, it is inv(T_start) @ T_end (Corke, Robotics, Vision and Control, chapter 2).
    """
    # The displacement in the frame of the poses (odometry or Gazebo world)
    dx = end_pose.x - start_pose.x
    dy = end_pose.y - start_pose.y
    # The same displacement in the robot's frame at the start, rotated by -theta: forward is its
    # part along the robot's heading (cos theta, sin theta), left its part along the robot's left
    # side (-sin theta, cos theta)
    cos_theta = math.cos(start_pose.theta)
    sin_theta = math.sin(start_pose.theta)
    forward = dx * cos_theta + dy * sin_theta
    left = -dx * sin_theta + dy * cos_theta
    turned = wrap(end_pose.theta - start_pose.theta)
    return RelativePose(forward, left, turned)

forward and left are the two rows of the matrix. atan2\((\sin\alpha, \cos\alpha)\) in wrap() gives the angle \(\alpha\) back in \(-\pi\) to \(\pi\), using the signs of both values.

Probabilistic Robotics describes the motion between two poses differently, as a rotation, a translation and a rotation: the odometry motion model, which filters such as amcl use to add noise to each part (Robot Motion). The test needs no noise, and it checks forward, left and turned directly, so it uses the relative pose.

The checks

test_forward() expects the robot to drive FORWARD_VELOCITY * DURATION, 0.2 m/s for 3 s, so 0.6 m straight ahead. This is the simplest case of the velocity motion model, \(v \cdot t\) with no turn (Robot Motion).

assertAlmostEqual(measured, expected, delta=d) passes when the two differ by at most d. The tolerances allow what the robots measure in Gazebo and reject the mistakes that were tried on purpose; smaller mistakes may pass. The loop checks the odometry and the true pose the same way, and the last check compares them (the three comparisons):

Check	Expected	Measured	Fails when
forward	0.6 ± 0.1 m	0.56 to 0.62 m	The robot doesn't move, or drives much too slow or too fast
left	0 ± 0.1 m	About 1 mm	It drives off to the side
turned	0 ± 0.2 rad	0.005 rad or less	Its heading changes, for example because one wheel turns slower
Odometry and true forward	Within 0.03 m	0.010 to 0.011 m apart	The controller's wheel radius doesn't match the model: a wheel_radius 10 % bigger or smaller than the configured 0.0325 m gave about 0.044 m and 0.076 m

A bigger mistake already fails the true pose's forward check: with a wheel_radius 20 % bigger than configured, the robot drove only 0.49 m.

Why the results differ from the commands explains why the robots drive 0.56 to 0.62 m instead of 0.6 m.

5. test_turn()

Lines 203–220. The highlighted lines are the ones that differ from test_forward():

def test_turn(self) -> None:
    odom_motion, true_motion = self.measure_motion(
        linear_velocity=0.0, angular_velocity=TURN_VELOCITY, duration=DURATION)
    motions = f'odometry {odom_motion}, true pose {true_motion}'
    # w * t = 1.5 rad to the left, on the spot. In Gazebo the robots turn less (DiffBot about
    # 1.3 rad, Remo 0.92 to 1.34 rad): Gazebo's wheel velocity controller
    # (diffbot_control/config/pid.yaml) has only a P gain, which leaves Remo's wheels at about
    # 80 % of the commanded speed, and the wheels slip. Until the gains are tuned (#116), the
    # tolerance is wide; a robot that turns right or not at all still fails
    expected_turn = TURN_VELOCITY * DURATION
    for motion in (odom_motion, true_motion):
        self.assertAlmostEqual(motion.turned, expected_turn, delta=1.0, msg=motions)
        # On the spot: its center drifts 2 to 4 cm in Gazebo
        self.assertAlmostEqual(math.hypot(motion.forward, motion.left), 0.0, delta=0.15,
                               msg=motions)
    # The wheels slip a little when turning on the spot (0.02 to 0.11 rad apart). With a
    # wheel_radius 15 % bigger than now, DiffBot's were 0.27 rad apart, but Remo's only 0.11
    self.assertAlmostEqual(odom_motion.turned, true_motion.turned, delta=0.25, msg=motions)

It commands the angular velocity instead of the linear one and expects 0.5 rad/s for 3 s, so 1.5 rad to the left; the controller's angular acceleration limit of 6 rad/s² hardly matters. On the spot means that the robot's center stays where it was. math.hypot(forward, left) is its distance from the start, the length of the hypotenuse of a right triangle with these two sides. The test starts wherever test_forward() left the robot; that doesn't matter, because it checks only the motion from there.

Check	Expected	Measured	Fails when
turned	1.5 ± 1.0 rad	DiffBot about 1.3 rad, Remo 0.92 to 1.34 rad	The robot turns right, for example with swapped wheel joints, or doesn't turn
Distance from the start	0 ± 0.15 m	Odometry below 0.01 m, true pose 0.02 to 0.04 m	It drives off instead of turning on the spot
Odometry and true turned	Within 0.25 rad	0.02 to 0.11 rad apart	They turn clearly differently. A wheel_radius 15 % bigger than configured gave 0.27 rad for DiffBot, which fails, but only 0.11 rad for Remo, which passes

The robots turn clearly less than 1.5 rad, and Remo's turn varies between runs, so the tolerance is wide until the simulation's wheel controllers are tuned (Why the results differ from the commands).

6. publishtest

When test_drive.py has ended, rostest stops everything and starts sim_drive.launch again for the second test node, publish_diffbot (step 1). publishtest checks the topics in its parameters in the test file: joint_states and the odometry must each get a message within 90 s. So it checks that both controllers started; test_drive.py doesn't use joint_states.

Why the results differ from the commands

The commands give an ideal expectation: 0.6 m straight ahead, 1.5 rad on the spot. The robots in Gazebo come close, but not exactly, and the odometry and the true pose differ a little. Three questions explain the numbers in Read the results and in the check tables.

Why the measured motion differs from the command

• Timing: drive() sends the command for 3 s of simulation time, but the last command stays in effect until stop(), nominally up to 50 ms longer: 1 cm or 0.025 rad more (the comment above COMMAND_RATE).
• Acceleration and braking: diff_drive_controller limits the acceleration (diffbot_control.yaml: 0.6 m/s² and 6 rad/s²), and it limits braking the same way, because min_acceleration defaults to the negative maximum. Speeding up to 0.2 m/s takes 0.33 s and covers 3.3 cm less than an instant start would. Braking after the zero command takes just as long and covers the same 3.3 cm more. measure_motion() measures after stop() has waited until the robot stands still, so the two cancel. The angular limit hardly matters for the turn.
• Wheel speed: in the simulation, each wheel's velocity is controlled by DefaultRobotHWSim's PID controller with the gains from pid.yaml, which has only a P gain. A controller with only a P gain can leave a speed error, especially under load. This is clearly visible when Remo turns on the spot: its wheels reach only about 80 % of the commanded speed, and their speed jitters (diffbot#116). DiffBot's odometry, which counts the wheel turns, shows 0.56 to 0.59 m instead of 0.6 m, and 1.37 rad instead of 1.5 rad. Slower wheels can contribute to that, as can what happens while the robot starts and stops, but the test doesn't measure these effects separately.

Why the odometry and the true pose differ

The odometry only knows how far the wheels turned, and converts that with the wheel radius and separation from diffbot_control.yaml. The true pose comes from the simulated robot. Two things make them differ:

• The configured geometry doesn't match the URDF. Both robots use diffbot_control.yaml, with a wheel radius of 0.0325 m and a wheel separation of 0.145 m. The URDFs have a wheel radius of 0.0331 m, and a separation of 0.135 m for DiffBot and 0.139 m for Remo. The simulated wheel rolls 1.8 % farther per turn than the odometry assumes, so the odometry's 0.580 m become 0.590 m.
• The odometry can't see slip. With the configured geometry, DiffBot's wheel turns give 1.37 rad, which is what its odometry reports. With the URDF's geometry, the same wheel turns would turn the robot about 1.50 rad, but the simulated robot turned only 1.26 rad: about 16 % of the wheels' motion slipped. When the robot drives straight, the wheels hardly slip.

The checks that compare the odometry and the true pose allow these differences. Three views of the motion explains why a wrong configuration can stay hidden in the odometry, and the checks what the agreement checks rejected.

Why the robots and repeated runs differ

• The models differ a lot. Both robots share the controller configuration and the wheel controller gains, but DiffBot's wheels weigh 0.5 kg each and use Gazebo's default contact settings. Remo's wheels weigh 32 g, with very stiff contacts and no contact damping (robot.gazebo.xacro in remo_description).
• The variation differs a lot. Over four runs, DiffBot's turn varied by about 0.01 rad, Remo's between 0.92 and 1.34 rad.
• A likely reason, not yet proven: Gazebo's physics computes the same result from the same inputs, but the test node's commands reach the simulation asynchronously, a few simulation steps earlier or later from run to run. Remo's light model with stiff contacts would turn such small differences into visibly different results. A run of ROS and Gazebo isn't guaranteed to be repeatable step for step.

diffbot#118 matches the geometry, masses and contact settings to each robot, and diffbot#116 then tunes the wheel controllers and narrows the turn's tolerance.

How to add a test

As an example, this adds a test that drives backward. It runs for both robots, because both test files run test_drive.py.

1. Write the test method. Add it to the TestDrive class in test_drive.py, next to test_forward(). The highlighted lines are the ones that differ from test_forward():

   def test_backward(self) -> None:
       odom_motion, true_motion = self.measure_motion(
           linear_velocity=-FORWARD_VELOCITY, angular_velocity=0.0, duration=DURATION)
       motions = f'odometry {odom_motion}, true pose {true_motion}'
       expected_forward = -FORWARD_VELOCITY * DURATION
       for motion in (odom_motion, true_motion):
           self.assertAlmostEqual(motion.forward, expected_forward, delta=0.1, msg=motions)
           self.assertAlmostEqual(motion.left, 0.0, delta=0.1, msg=motions)
           self.assertAlmostEqual(motion.turned, 0.0, delta=0.2, msg=motions)
       self.assertAlmostEqual(odom_motion.forward, true_motion.forward, delta=0.03, msg=motions)
   

   A negative linear velocity drives backward, so forward is negative: −0.6 m are expected. The tolerances are copied from test_forward() for a start. The method's name must start with test, so that unittest runs it. unittest runs it first, before test_forward(), because of the alphabetical order; that's fine, because each test checks only its own motion.

2. Run it for both robots, as in Run it:

   rostest diffbot_gazebo sim_drive_diffbot.test
   rostest diffbot_gazebo sim_drive_remo.test
   

   The summary now lists test_backward too, for example [diffbot_gazebo.rosunit-drive_diffbot/test_backward][passed].

3. Look at the measured values in the result file, as in Read the results. The first two lines are now test_backward()'s motions. In our run, DiffBot drove −0.57 m by the odometry and −0.58 m by the true pose, and turned 0.1 rad to the right on the way, more than when it drives forward. Remo drove −0.58 and −0.59 m, almost without turning.

4. Choose the tolerances. Each tolerance should allow what both robots measure and reject the mistakes the test should catch. To see what a mistake gives, make one on purpose: for example, make wheel_radius in diffbot_control.yaml 10 % bigger, run the test again, look at the values, and undo the change. Then set each tolerance between the two, and write in a comment what you measured.

5. Keep the file's style: PEP 8 with ROS 2's line length of 99 characters (ament_flake8's settings), type hints, and named constants instead of bare numbers in the commands.

A test that needs another world or robot needs its own .test file. Register it with add_rostest in the package's CMakeLists.txt, and give it its own gazebo_port (the comment in sim_drive.launch explains why).

Comments

Comments are GitHub Discussions, shown with giscus. Loading them connects to giscus.app (hosted by Vercel, USA) and GitHub (USA); you need a GitHub account to comment.

Show comments