Skip to content

Test: The EKF in Gazebo

The test proves one thing in the simulation: when the wheel odometry reports a wrong turn, the EKF's heading \(\theta\) still follows the robot's true turn, because the filter takes the yaw rate \(\omega_z\) from the IMU. The test makes the wheel odometry wrong on purpose. It gives the drive controller a wheel separation \(b_c\) that is 25 % larger than the simulated robot's wheel separation \(b\). The same simulation also checks that only the EKF publishes the transform odom → base_footprint. Both simulated robots run the test, DiffBot and Remo. The test doesn't show how the filter behaves on the real robot. What the test doesn't show lists the differences.

test_ekf.py has two tests:

Test What it checks Limit
test_ekf_follows_the_true_turn_with_a_wrong_wheel_separation In a left turn of 3 s: the IMU sample validator forwards the IMU's samples, the wheel odometry's turn falls behind the true turn, and the EKF's turn is the true turn more than 45 Hz, more than 0.15 rad behind, within 0.05 rad
test_ekf_is_the_only_odom_tf_publisher Only the node /diffbot/ekf_localization publishes the transform odom → base_footprint on the topic /tf exactly one node

The turn from above

The figure shows the turn test from above, with the values of a saved run and one drawing for each simulated robot. In a drawing, the robot starts with its heading along the dashed line to the right and turns left, counterclockwise. Each arc is one turn from this start: the robot's true turn \(\Delta\theta_\text{true}\) from Gazebo, the turn \(\Delta\theta_\text{EKF}\) of the EKF's estimate and the turn \(\Delta\theta_\text{odom}\) of the wheel odometry. The label at the end of an arc is the arc's name in the line that the test prints, true, EKF or odometry, with the measured turn. The robot is drawn as it stands after the turn, and the line on its body points forward. The small green sector at the end of the EKF's arc is where this arc has to end: 0.05 rad to each side of the true turn, as the label "allowed ±0.05 rad" in the drawing's upper left says. The line above a drawing gives the robot's two wheel separations \(b\) and \(b_c\), and the list under the drawing gives the results of the two checks on the turn.

DiffBot

Wheel separation: b = 0.138 m on the simulated robot, bc = 0.1725 m in the drive controller

The left turn of the simulated DiffBot from above: the robot turned 1.849 rad, the EKF's estimate 1.844 rad and the wheel odometry 1.479 rad start DiffBot: the EKF's turn has to end between 1.799 rad and 1.899 rad, 0.05 rad to each side of the true turn DiffBot: the robot turned 1.849 rad in Gazebo DiffBot: the EKF's estimate turned 1.844 rad DiffBot: the wheel odometry turned 1.479 rad true 1.849 rad EKF 1.844 rad odometry 1.479 rad allowed ±0.05 rad
  • ✓The EKF follows: its turn is 0.005 rad from the true turn. Allowed: 0.05 rad
  • ✓The wheel odometry falls behind: by 0.370 rad. Required: more than 0.15 rad

Remo

Wheel separation: b = 0.144 m on the simulated robot, bc = 0.18 m in the drive controller

The left turn of the simulated Remo from above: the robot turned 1.873 rad, the EKF's estimate 1.870 rad and the wheel odometry 1.499 rad start Remo: the EKF's turn has to end between 1.823 rad and 1.923 rad, 0.05 rad to each side of the true turn Remo: the robot turned 1.873 rad in Gazebo Remo: the EKF's estimate turned 1.870 rad Remo: the wheel odometry turned 1.499 rad true 1.873 rad EKF 1.870 rad odometry 1.499 rad allowed ±0.05 rad
  • ✓The EKF follows: its turn is 0.003 rad from the true turn. Allowed: 0.05 rad
  • ✓The wheel odometry falls behind: by 0.374 rad. Required: more than 0.15 rad
  • true: the robot's turn in Gazebo, Δθtrue
  • EKF: the turn of the EKF's estimate, ΔθEKF
  • odometry: the turn of the wheel odometry, Δθodom
  • where the EKF's arc has to end: EKF_TOLERANCE, 0.05 rad, to each side of the true turn
The left turn of test_ekf.py from above, in a run at the commit 3ef4a91. Each arc is one turn, from the dashed start heading.

The table lists the values of the figure, one column for each simulated robot. The run was in the dev container's image, at the commit 3ef4a91 of diffbot#154:

Measurement DiffBot Remo
Wheel separation \(b\) of the simulated robot 0.138 m 0.144 m
Controller's wheel separation \(b_c\) 0.1725 m 0.18 m
True turn \(\Delta\theta_\text{true}\) 1.849 rad 1.873 rad
Wheel odometry's turn \(\Delta\theta_\text{odom}\) 1.479 rad 1.499 rad
EKF's turn \(\Delta\theta_\text{EKF}\) 1.844 rad 1.870 rad
Rate of the validated IMU samples 49.7 Hz 50.0 Hz

The two checks on the turn compare these turns. For DiffBot, they work out as follows:

  • The EKF follows the true turn: \(|\Delta\theta_\text{EKF} - \Delta\theta_\text{true}| = |1.844\ \text{rad} - 1.849\ \text{rad}| = 0.005\ \text{rad}\). The test allows at most 0.05 rad, the green sector.
  • The wheel odometry falls behind: \(\Delta\theta_\text{true} - \Delta\theta_\text{odom} = 1.849\ \text{rad} - 1.479\ \text{rad} = 0.370\ \text{rad}\). The test requires more than 0.15 rad.

Run it

In the dev container, in the built workspace, run the test for each simulated robot:

1
2
rostest diffbot_gazebo sim_ekf_diffbot.test
rostest diffbot_gazebo sim_ekf_remo.test

A pass ends with * RESULT: SUCCESS under SUMMARY, after one [passed] line for each of the two tests. CI runs both files with the repository's other tests, as Testing and CI describes.

The output of a passing run, and the line that the test prints

Run it and Read the results on Simulation Test explain rostest's options, its output and its result files. A passing run of sim_ekf_diffbot.test ends with:

1
2
3
4
5
6
7
8
[diffbot_gazebo.rosunit-ekf_diffbot/test_ekf_follows_the_true_turn_with_a_wrong_wheel_separation][passed]
[diffbot_gazebo.rosunit-ekf_diffbot/test_ekf_is_the_only_odom_tf_publisher][passed]

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

The test prints the measured turn in one line, which rostest copies into the result file rosunit-ekf_diffbot.xml. In the saved run, the line was:

1
controller's wheel separation 0.1725 m; turned: true 1.849 rad, odometry 1.479 rad, EKF 1.844 rad; validated IMU 49.7 Hz

What a pass proves and what a failure means

The test gives the controller a wheel separation 25 % too large, so the wheel odometry reports about 20 % less than the true turn. A pass proves that in the simulation the EKF's turn lies within 0.05 rad of the true turn, the wheel odometry's turn falls more than 0.15 rad behind, the validator forwards the IMU's samples at more than 45 Hz, and only the EKF publishes the transform odom → base_footprint.

A failure names the test and the check. The turn test checks four things in a fixed order, from the setup to the EKF's turn, so the first failing check says how far the run got. A missing source of a heading fails before the turn, with the names of the silent sources.

What the test doesn't show

  • The real gyroscope: the simulated IMU has noise but no bias and no mounting error.
  • The real robot's rates and delays: in the simulation, both inputs are stamped with the simulation's time. The rate check counts the validated IMU samples over the turn, so it doesn't show that each message arrived on time.
  • Other errors of the wheels: one constant factor on a turn on the spot, no varying slip, no driving forward.
  • The position and turns of more than \(\pi\): the test compares the three turns only.
The details
  • The real gyroscope: the simulated IMU adds random noise to the true yaw rate and reports the matching variance \(\sigma^2\). It has no bias and no mounting error. The simulated IMU describes the sensor.
  • The real robot's rates and delays: in the simulation, the wheel odometry and the IMU are both configured for 50 Hz and stamped with simulation time. The saved DiffBot run measured 49.7 Hz of validated IMU samples. On the robot, the wheel odometry arrives at most 10 times per second, as The other settings on the Localization page explains. How it's tested on the same page lists what the tests don't show about the robot's rates and delays.
  • Other errors of the wheels: the test's error is one constant factor on a turn on the spot. The test has no slip that comes and goes and no driving forward.
  • The position: the test compares the three turns only, not the positions \(x\) and \(y\).
  • Turns of more than \(\pi\): turned() returns a change of the heading between \(-\pi\) and \(\pi\).

What the simulation starts

sim_ekf_diffbot.test and sim_ekf_remo.test start the simulation with the EKF on and the drive controller's wheel separation set to 1.25 times the robot's, and run test_ekf.py.

The rostest file and the launch files

Two rostest files run the test, sim_ekf_diffbot.test for the simulated DiffBot and sim_ekf_remo.test for the simulated Remo. A rostest file is a launch file with a <test> element. rostest starts the file's nodes, runs the test node and stops everything when the test node ends, as the test file on Simulation Test explains. The excerpt shows what DiffBot's file starts, from sim_ekf_diffbot.test, lines 9–21:

sim_ekf_diffbot.test: the simulation with the EKF, the wheel separation and the test node
 9
10
11
12
13
14
15
16
17
18
19
20
21
<include file="$(find diffbot_gazebo)/test/sim_drive.launch">
    <arg name="model" value="diffbot"/>
    <arg name="gazebo_port" value="11353"/>
    <arg name="ekf" value="true"/>
</include>
<!-- After geometry.yaml, which sim_drive.launch loads: this value wins -->
<param name="/diffbot/mobile_base_controller/wheel_separation" value="0.1725"/>

<test test-name="ekf_diffbot" pkg="diffbot_gazebo" type="test_ekf.py" time-limit="180">
    <!-- The robot whose geometry.yaml the test compares the controller's wheel separation
         with -->
    <param name="model" value="diffbot"/>
</test>
  • sim_drive.launch: starts the simulation that the other simulation tests use too. ekf set to true switches the EKF on.
  • gazebo_port: is the port of this test file's Gazebo server. Each simulation test file has a port of its own, because catkin runs the test files at the same time.
  • wheel_separation: the <param> line comes after the include, so its value replaces the value that the included files loaded from the robot's geometry.yaml. 0.1725 m is 1.25 times DiffBot's 0.138 m. sim_ekf_remo.test sets 0.18 m, 1.25 times Remo's 0.144 m.
  • The test node: ekf_diffbot runs test_ekf.py. The node's private parameter model names the robot whose geometry.yaml the test reads.

The next excerpt shows how ekf reaches the drive controller's launch file, from sim_drive.launch, lines 15–25:

sim_drive.launch: Gazebo without a window, and the controllers with the ekf argument
15
16
17
18
19
20
21
22
23
24
25
<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"/>
    <arg name="ekf" value="$(arg ekf)"/>
</include>

diffbot_view.launch starts Gazebo's server and spawns the robot from its URDF model. diffbot_control.launch loads the drive controller's configuration, with the robot's geometry.yaml, and starts the controllers. With ekf true, diffbot_control.launch also sets the controller's enable_odom_tf to false and includes ekf.launch, which starts the IMU sample validator and the EKF, as Which launch file starts what shows.

Why a wrong wheel separation

With a wheel separation \(b_c\) 1.25 times the robot's \(b\), the drive controller turns the robot 25 % more than its wheel odometry reports. The IMU measures the true turn, so the EKF's turn follows the robot and the wheel odometry's doesn't.

The wheel separation in the drive controller, with the numbers of the test

The drive controller, diff_drive_controller, uses its wheel_separation parameter twice. It turns a commanded yaw rate into two wheel speeds, and it turns the wheels' movement back into the odometry's yaw rate. \(b\) is the wheel separation of the simulated robot, the distance between its two wheels in the URDF model. \(b_c = 1.25\,b\) is the controller's value. The factor 1.25 is the test's SEPARATION_SCALE. For a commanded yaw rate \(\omega_\text{cmd}\), the controller makes the right wheel's speed \(v_r\) larger than the left wheel's speed \(v_l\) by

\[ v_r - v_l = \omega_\text{cmd}\, b_c \]

The wheels of the simulated robot are \(b\) apart, so the robot turns with the yaw rate

\[ \omega_z = \frac{v_r - v_l}{b} = \frac{b_c}{b}\,\omega_\text{cmd} = 1.25\,\omega_\text{cmd} \]

The wheel odometry divides the same speed difference by \(b_c\) and reports \(\omega_\text{cmd}\). So the robot turns 25 % more than its wheel odometry says. This constant error stands in for the errors of a real robot's wheels, slip and a wheel separation that is a little off. The simulated IMU measures the robot's true yaw rate \(\omega_z\). A filter that follows the IMU ends at the true heading, and a filter that follows the wheels ends where the wheel odometry ends.

The test commands \(\omega_\text{cmd}\) = 0.5 rad/s (TURN_VELOCITY) for 3 s (DURATION). For these round numbers, the wheel odometry reports 1.5 rad and the robot turns 1.875 rad.

In a run, the measured turns are a little smaller than these round numbers, as the figure shows. The true turn stays 1.25 times the wheel odometry's turn.

The constants

The constants of test_ekf.py

The excerpt shows the topics, the names and the numbers of the test, from test_ekf.py, lines 41–69. STARTUP_TIMEOUT and POLL_INTERVAL are seconds of wall time, the computer's clock. DURATION and SETTLE are seconds of simulation time, Gazebo's clock.

test_ekf.py: the constants
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
CMD_VEL_TOPIC = '/diffbot/mobile_base_controller/cmd_vel'
WHEEL_ODOMETRY_TOPIC = '/diffbot/mobile_base_controller/odom'
FILTERED_TOPIC = '/diffbot/odometry/filtered'
VALIDATED_IMU_TOPIC = '/diffbot/imu/data_validated'
MODEL_STATES_TOPIC = '/gazebo/model_states'
MODEL_NAME = 'diffbot'  # the spawned model's name, for DiffBot and Remo (diffbot_view.launch)
EKF_NODE = '/diffbot/ekf_localization'
ODOM_TF_PARAMETER = '/diffbot/mobile_base_controller/enable_odom_tf'
WHEEL_SEPARATION_PARAMETER = '/diffbot/mobile_base_controller/wheel_separation'
COMMAND_QUEUE_SIZE = 1  # only the latest command matters
MODEL_STATES_QUEUE_SIZE = 1  # only the latest true pose matters
IMU_QUEUE_SIZE = 100  # messages, 2 s at IMU_RATE: the test counts them all

TURN_VELOCITY = 0.5  # rad/s, commanded, to the left
DURATION = 3.0  # s
COMMAND_RATE = 20  # Hz
IMU_RATE = 50.0  # Hz
# The controller's wheel separation over the robot's, as the rostest file sets it
SEPARATION_SCALE = 1.25
STARTUP_TIMEOUT = 120.0  # s of wall time
POLL_INTERVAL = 0.1  # s of wall time between checks for the controller and the robot
SETTLE = 1.0  # s of simulation time for the robot to stop before the test reads the headings
# Limits
SEPARATION_TOLERANCE = 1e-6  # m, between the controller's separation and the scaled one
MIN_IMU_RATE_SHARE = 0.9  # the share of IMU_RATE that the validated IMU must reach at least
# rad, the least by which the odometry's turn must fall behind the true one. With the scaled
# separation, the odometry reports about 1 / SEPARATION_SCALE of the true turn
MIN_ODOMETRY_ERROR = 0.15
EKF_TOLERANCE = 0.05  # rad, how far the EKF's turn may differ from the true one

Reading the three headings

The class HeadingObserver subscribes to the true pose from Gazebo, the wheel odometry and the EKF's estimate, and keeps the latest heading \(\theta\) of each. It also counts the IMU samples that the validator forwards.

yaw(), turned() and HeadingObserver

The test compares three headings \(\theta\) of the robot: the true heading from Gazebo, the heading of the wheel odometry and the heading of the EKF's estimate. Each of these three sources publishes on a topic. The class HeadingObserver subscribes to the topics and keeps the latest heading of each source. Waiting until everything runs and The turn show where the test reads them.

A message of these topics carries a pose, not a heading. The excerpt shows the two functions that turn a pose into a heading and two headings into a turn, from test_ekf.py, lines 86–94:

test_ekf.py: yaw() and turned()
86
87
88
89
90
91
92
93
94
def yaw(pose: Pose) -> float:
    """Return the heading of pose, its rotation about z (rad)."""
    orientation = pose.orientation
    return euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])[2]


def turned(start: float, end: float) -> float:
    """Return the change of heading from start to end (rad), in -pi..pi."""
    return math.atan2(math.sin(end - start), math.cos(end - start))
  • yaw(): a pose stores its orientation as a quaternion. euler_from_quaternion() returns roll, pitch and yaw, and yaw() returns the third, the heading \(\theta\).
  • turned(): returns the change of the heading from start to end, between \(-\pi\) and \(\pi\). The turn of this test is less than 2 rad, so the result is the turn itself.

The next excerpt shows the type Headings and how a HeadingObserver starts, from test_ekf.py, lines 106–134:

test_ekf.py: Headings and HeadingObserver.__init__()
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
class Headings(NamedTuple):
    """The robot's heading at one moment, by each source (rad)."""

    true: float  # from Gazebo
    odometry: float  # from the drive controller's odometry
    ekf: float  # from the EKF's estimate


class HeadingObserver:
    """Keep the latest heading of each source, and count the validated IMU samples.

    The observer counts from its creation.
    """

    def __init__(self) -> None:
        # rospy calls each callback in a thread of its own, and the test reads in another. The
        # lock gives the test one consistent copy of the three headings. They are the latest
        # of each source, not headings of the same instant
        self.lock = threading.Lock()
        self.true_heading: Optional[float] = None
        self.odometry_heading: Optional[float] = None
        self.ekf_heading: Optional[float] = None
        self.sample_count = 0
        rospy.Subscriber(WHEEL_ODOMETRY_TOPIC, Odometry, self.on_wheel_odometry)
        rospy.Subscriber(FILTERED_TOPIC, Odometry, self.on_filtered_odometry)
        rospy.Subscriber(MODEL_STATES_TOPIC, ModelStates, self.on_model_states,
                         queue_size=MODEL_STATES_QUEUE_SIZE)
        rospy.Subscriber(VALIDATED_IMU_TOPIC, Imu, self.on_validated_imu,
                         queue_size=IMU_QUEUE_SIZE)
  • Headings: the robot's heading at one moment by each source, in rad. true is from Gazebo, odometry from the drive controller's wheel odometry and ekf from the EKF's estimate.
  • true_heading, odometry_heading and ekf_heading: the observer's latest heading of each source. Each is None until the first message of its source arrives.
  • sample_count: the number of samples that the IMU sample validator forwarded since the observer was created.
  • The four subscribers: they read the drive controller's wheel odometry (WHEEL_ODOMETRY_TOPIC), the EKF's estimate (FILTERED_TOPIC), Gazebo's model states (MODEL_STATES_TOPIC), which carry the robot's true pose, and the validator's output (VALIDATED_IMU_TOPIC). The subscriber of the model states keeps one message, MODEL_STATES_QUEUE_SIZE, because only the latest true pose matters. The subscriber of the validated samples keeps up to 100 messages, IMU_QUEUE_SIZE, because the test counts every sample.
  • lock: several threads use the observer's four values, the three headings and sample_count. rospy has one receiving thread for each publisher connection of a subscribed topic, and calls the subscriber's callback in that thread. Each of the four topics has one publisher, so the four callbacks run in four threads. The test itself runs in the test node's main thread, which calls setUpClass() and the tests and reads the observer. The lock lets one thread at a time read or change the four values. So the test gets the three headings as one copy, and no callback changes a heading while the test copies them. The three headings are the latest of each source, not headings of the same instant.

Each callback stores what the test needs from one message. The excerpt shows the four callbacks, from test_ekf.py, lines 136–155:

test_ekf.py: the callbacks of HeadingObserver
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
def on_wheel_odometry(self, message: Odometry) -> None:
    """Note the heading of the controller's odometry."""
    with self.lock:
        self.odometry_heading = yaw(message.pose.pose)

def on_filtered_odometry(self, message: Odometry) -> None:
    """Note the heading of the EKF's estimate."""
    with self.lock:
        self.ekf_heading = yaw(message.pose.pose)

def on_model_states(self, message: ModelStates) -> None:
    """Note the robot's true heading in Gazebo."""
    if MODEL_NAME in message.name:
        with self.lock:
            self.true_heading = yaw(message.pose[message.name.index(MODEL_NAME)])

def on_validated_imu(self, message: Imu) -> None:
    """Count a sample that the validator forwarded."""
    with self.lock:
        self.sample_count += 1
  • on_wheel_odometry() and on_filtered_odometry(): both topics carry nav_msgs/Odometry messages. Each callback stores the heading of the message's pose, yaw(message.pose.pose).
  • on_model_states(): Gazebo publishes the poses of all models in one message. The callback takes the pose of the model named MODEL_NAME. Gazebo's model of the robot is named diffbot for DiffBot and for Remo.
  • on_validated_imu(): counts a sample that the IMU sample validator forwarded.
  • with self.lock: each callback takes the lock before it changes its value and gives the lock back at the end of the with block. While another thread holds the lock, the callback waits.

The test reads the observer through three methods, and each holds the lock while it reads. The excerpt shows them, from test_ekf.py, lines 157–175:

test_ekf.py: latest(), silent_sources() and validated_sample_count()
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
def latest(self) -> Optional[Headings]:
    """Return the latest heading of each source, or None until each has sent one."""
    with self.lock:
        if (self.true_heading is None or self.odometry_heading is None
                or self.ekf_heading is None):
            return None
        return Headings(self.true_heading, self.odometry_heading, self.ekf_heading)

def silent_sources(self) -> List[str]:
    """Return the sources that haven't sent a heading yet, for a failure message."""
    with self.lock:
        headings = (('Gazebo', self.true_heading), ('odometry', self.odometry_heading),
                    ('EKF', self.ekf_heading))
        return [source for source, heading in headings if heading is None]

def validated_sample_count(self) -> int:
    """Return the number of validated IMU samples so far."""
    with self.lock:
        return self.sample_count
  • latest(): returns the latest heading of each source as one Headings. It returns None until each of the three sources has sent a heading.
  • silent_sources(): returns the names of the sources that haven't sent a heading yet, out of Gazebo, odometry and EKF. setUpClass() puts them into its failure message.
  • validated_sample_count(): returns the number of validated IMU samples so far.

Waiting until everything runs

setUpClass() starts the observers and the command publisher and waits, at most STARTUP_TIMEOUT, 120 s of wall time, until each of the three sources has sent a heading and the drive controller listens to the commands.

setUpClass() and ready()

unittest calls setUpClass() once, before the first test of the class. setUpClass() creates the test's node, a HeadingObserver, the observer of the topic /tf and the publisher of the velocity commands. Then it waits until ready() returns true. The excerpt shows both methods, from test_ekf.py, lines 185–204:

test_ekf.py: setUpClass() and ready()
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
@classmethod
def setUpClass(cls) -> None:
    rospy.init_node('test_ekf')
    cls.heading_observer = HeadingObserver()
    cls.odom_tf_observer = OdomTfObserver()
    cls.command = rospy.Publisher(CMD_VEL_TOPIC, Twist, queue_size=COMMAND_QUEUE_SIZE)
    deadline = time.monotonic() + STARTUP_TIMEOUT
    while not cls.ready():
        if time.monotonic() > deadline:
            raise AssertionError(
                'not ready after {} s: no heading yet from {}, {} subscribers to the '
                'commands'.format(STARTUP_TIMEOUT, cls.heading_observer.silent_sources(),
                                  cls.command.get_num_connections()))
        time.sleep(POLL_INTERVAL)

@classmethod
def ready(cls) -> bool:
    """Return whether a heading of each source arrived and the controller takes commands."""
    return (cls.heading_observer.latest() is not None
            and cls.command.get_num_connections() > 0)
  • cls.heading_observer: from its creation on, the HeadingObserver keeps the latest headings and counts the validated IMU samples.
  • cls.odom_tf_observer: OdomTfObserver() starts to count who publishes the transform odom → base_footprint, for the second test. The Transform's Publisher explains this observer.
  • cls.command: publishes the velocity commands on CMD_VEL_TOPIC, the drive controller's command topic.
  • ready(): is true when latest() returns headings, so a heading arrived from each of the three sources, and the drive controller has connected to the command topic. ROS loses a command that is published before this connection exists.
  • STARTUP_TIMEOUT: after 120 s of wall time, setUpClass() raises an AssertionError. Its message names the sources that sent no heading, from silent_sources(), and the number of subscribers to the commands.

The turn

measure_turn() turns the robot on the spot at 0.5 rad/s for 3 s of simulation time, stops it, and returns the turn that each source saw and the rate of the validated IMU samples.

measure_turn() and TurnMeasurement

measure_turn() turns the robot on the spot and returns what each source saw. It reads the headings with latest_headings(). The excerpt shows both methods, from test_ekf.py, lines 206–233:

test_ekf.py: latest_headings() and measure_turn()
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
def latest_headings(self) -> Headings:
    """Return the latest heading of each source. setUpClass() waited for the first ones."""
    headings = self.heading_observer.latest()
    assert headings is not None
    return headings

def measure_turn(self) -> TurnMeasurement:
    """Turn the robot on the spot for DURATION, stop it, and return what each source saw."""
    start = self.latest_headings()
    sample_count = self.heading_observer.validated_sample_count()
    started = rospy.Time.now()
    turn = Twist()
    turn.angular.z = TURN_VELOCITY
    rate = rospy.Rate(COMMAND_RATE)
    end = started + rospy.Duration(DURATION)
    try:
        while rospy.Time.now() < end and not rospy.is_shutdown():
            self.command.publish(turn)
            rate.sleep()
        imu_rate = ((self.heading_observer.validated_sample_count() - sample_count)
                    / (rospy.Time.now() - started).to_sec())
    finally:
        self.command.publish(Twist())
    rospy.sleep(SETTLE)
    stopped = self.latest_headings()
    return TurnMeasurement(turned(start.true, stopped.true),
                           turned(start.odometry, stopped.odometry),
                           turned(start.ekf, stopped.ekf), imu_rate)

latest_headings() returns what latest() of the HeadingObserver returns. Its assert stops the test if that is None. After setUpClass(), which waited for a heading of each source, latest() returns a Headings.

  1. measure_turn() reads the three headings with latest_headings() into start, notes the count of validated IMU samples and reads the simulation time.
  2. For DURATION, 3 s of simulation time, it publishes a Twist command with angular.z set to TURN_VELOCITY, 0.5 rad/s, at COMMAND_RATE, 20 times per second. A positive yaw rate is a left turn. The command has no forward speed, so the robot turns on the spot.
  3. It computes the rate of the validated IMU samples: the number of samples that validated_sample_count() counted during the turn, divided by the time that the turn took.
  4. In the finally block, it publishes a command of zero. The robot stops also when an error ends the loop early.
  5. It waits SETTLE, 1 s of simulation time, until the robot stands.
  6. It reads the three headings again into stopped and returns a TurnMeasurement. For each source, turned() gives the turn from the heading in start to the heading in stopped, for example turned(start.true, stopped.true).

The excerpt shows the result's type, from test_ekf.py, lines 72–83:

test_ekf.py: TurnMeasurement
72
73
74
75
76
77
78
79
80
81
82
83
class TurnMeasurement(NamedTuple):
    """What one commanded turn did, by each source of the heading."""

    true_turn: float  # rad, from Gazebo
    odometry_turn: float  # rad, from the drive controller's odometry
    ekf_turn: float  # rad, from the EKF's estimate
    validated_imu_rate: float  # Hz, of the samples that the validator forwarded during the turn

    def report(self) -> str:
        """Return the three turns as text, for the test's output and its failure messages."""
        return 'true {:.3f} rad, odometry {:.3f} rad, EKF {:.3f} rad'.format(
            self.true_turn, self.odometry_turn, self.ekf_turn)

report() returns the three turns as one line of text. The test prints this line and puts it into the message of a failed check.

The checks on the turn

The excerpt shows the test method, from test_ekf.py, lines 271–281. The method first reads the controller's wheel separation from the parameter server and checks it, so a wrong setup fails before the robot is commanded to turn. Then the method measures one turn and makes the other three assertions:

test_ekf.py: the turn test
271
272
273
274
275
276
277
278
279
280
281
def test_ekf_follows_the_true_turn_with_a_wrong_wheel_separation(self) -> None:
    separation = rospy.get_param(WHEEL_SEPARATION_PARAMETER)
    self.assert_wheel_separation_is_scaled(separation)
    measurement = self.measure_turn()
    print("controller's wheel separation {:.4f} m; turned: {}; validated IMU {:.1f} Hz".format(
        separation, measurement.report(), measurement.validated_imu_rate))
    self.assert_validated_imu_rate(measurement)
    # The scaled separation shows in the odometry, which reports about 1 / 1.25 of the true
    # turn, and not in the EKF's estimate, which follows the IMU
    self.assert_odometry_falls_behind(measurement)
    self.assert_ekf_follows_the_true_turn(measurement)

In the list, \(\Delta\theta_\text{true}\) is the robot's true turn, \(\Delta\theta_\text{odom}\) the wheel odometry's turn and \(\Delta\theta_\text{EKF}\) the turn of the EKF's estimate, as in the figure.

  1. The wheel separation is the scaled one, assert_wheel_separation_is_scaled(): the parameter /diffbot/mobile_base_controller/wheel_separation is SEPARATION_SCALE times the wheel_separation in the robot's geometry.yaml, within SEPARATION_TOLERANCE, 10⁻⁶ m. This check shows that the rostest file's value reached the controller's parameter and still fits the robot's geometry.
  2. The validator forwards the IMU, assert_validated_imu_rate(): during the turn, the samples on the topic /diffbot/imu/data_validated arrived at more than MIN_IMU_RATE_SHARE times IMU_RATE, 0.9 · 50 Hz = 45 Hz. Without the validated samples, the EKF would have only the wheel odometry.
  3. The wheel odometry falls behind, assert_odometry_falls_behind(): \(\Delta\theta_\text{true} - \Delta\theta_\text{odom}\) is more than MIN_ODOMETRY_ERROR, 0.15 rad. This check shows that the error that the test needs is there. With the scaled separation, the wheel odometry reports 1 / 1.25 of the true turn, so the difference is a fifth of the true turn.
  4. The EKF follows the true turn, assert_ekf_follows_the_true_turn(): \(|\Delta\theta_\text{EKF} - \Delta\theta_\text{true}|\) is at most EKF_TOLERANCE, 0.05 rad.

Checks 3 and 4 compare the turns. The excerpt shows them, from test_ekf.py, lines 250–259:

test_ekf.py: the checks of the turns
250
251
252
253
254
255
256
257
258
259
def assert_odometry_falls_behind(self, measurement: TurnMeasurement) -> None:
    """Assert that the odometry reports a clearly smaller left turn than the true one."""
    self.assertGreater(measurement.true_turn - measurement.odometry_turn, MIN_ODOMETRY_ERROR,
                       'the odometry should fall behind the true turn: '
                       + measurement.report())

def assert_ekf_follows_the_true_turn(self, measurement: TurnMeasurement) -> None:
    """Assert that the EKF's turn is the true one within EKF_TOLERANCE."""
    self.assertAlmostEqual(measurement.ekf_turn, measurement.true_turn, delta=EKF_TOLERANCE,
                           msg='the EKF should follow the true turn: ' + measurement.report())

Both messages end with measurement.report(), the three turns, so a failure shows the measured values.

The first two assertion methods and the robot's own wheel separation

The next excerpt shows the first two assertion methods, from test_ekf.py, lines 235–248:

test_ekf.py: the first two assertions
235
236
237
238
239
240
241
242
243
244
245
246
247
248
def assert_wheel_separation_is_scaled(self, separation: float) -> None:
    """Assert that the controller's separation is SEPARATION_SCALE times the robot's."""
    model = rospy.get_param('~model')
    nominal = nominal_wheel_separation(model)
    self.assertAlmostEqual(
        separation, SEPARATION_SCALE * nominal, delta=SEPARATION_TOLERANCE,
        msg="the controller's wheel separation should be {} times {}'s {} m".format(
            SEPARATION_SCALE, model, nominal))

def assert_validated_imu_rate(self, measurement: TurnMeasurement) -> None:
    """Assert that the validator forwarded the IMU's samples at nearly IMU_RATE."""
    self.assertGreater(
        measurement.validated_imu_rate, MIN_IMU_RATE_SHARE * IMU_RATE,
        'the validator forwarded the IMU at {:.1f} Hz'.format(measurement.validated_imu_rate))

assert_wheel_separation_is_scaled() gets the robot's own value from nominal_wheel_separation(). The excerpt shows this function, from test_ekf.py, lines 97–103:

test_ekf.py: nominal_wheel_separation()
 97
 98
 99
100
101
102
103
def nominal_wheel_separation(model: str) -> float:
    """Return the wheel separation (m) in the model's geometry.yaml of diffbot_control."""
    path = os.path.join(rospkg.RosPack().get_path('diffbot_control'), 'config', model,
                        'geometry.yaml')
    with open(path) as geometry_file:
        geometry = yaml.safe_load(geometry_file)
    return float(geometry['diffbot']['mobile_base_controller']['wheel_separation'])

The turn from above lists what the test measured in a run, and works out checks 3 and 4 with these values.

Who publishes the transform

With the EKF on, the EKF has to publish the transform odom → base_footprint, and the drive controller must not. The test counts the transform's publishers on /tf for 3 s and expects only /diffbot/ekf_localization, and the controller's enable_odom_tf false.

The test

With the EKF on, the EKF has to publish the transform odom → base_footprint, and the drive controller must not. The excerpt shows the second test, from test_ekf.py, lines 261–269:

test_ekf.py: only the EKF publishes odom → base_footprint
261
262
263
264
265
266
267
268
269
def test_ekf_is_the_only_odom_tf_publisher(self) -> None:
    self.assertTrue(self.odom_tf_observer.wait_for_first_transform(STARTUP_TIMEOUT),
                    'nobody published odom → base_footprint')
    rospy.sleep(OBSERVE)
    counts = self.odom_tf_observer.counts()
    print('odom → base_footprint published by: {}'.format(counts))
    self.assertEqual([EKF_NODE], list(counts),
                     'publishers of odom → base_footprint: {}'.format(counts))
    self.assertFalse(rospy.get_param(ODOM_TF_PARAMETER))
  1. The test waits until self.odom_tf_observer has seen a first transform odom → base_footprint, at most STARTUP_TIMEOUT.
  2. It waits OBSERVE, 3 s of simulation time, while the observer keeps counting.
  3. It expects exactly one publishing node in the observer's counts, EKF_NODE, which is /diffbot/ekf_localization.
  4. It expects the controller's parameter enable_odom_tf, which ODOM_TF_PARAMETER names, to be false.

This observer counts from setUpClass() on. unittest runs a class's test methods in alphabetical order, so the turn test runs first, and the count covers the turn too. In the saved run, the observer counted 375 transforms for DiffBot and 371 for Remo, all from /diffbot/ekf_localization. The Transform's Publisher explains the observer and the same check without the EKF.

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.