Tests: The IMU Sample Validator
test_imu_validator.py tests the IMU sample validator one decision at a time. Its 30 unit tests call the validator's Python class and functions directly, without a ROS master, a node or a topic. They check which samples the validator accepts, the reason for each sample that it rejects, the silence and the level that its diagnostics report, and the counts of a report. The stepped-clock test runs the node together with the EKF. This page belongs to the tests of the Localization.
One sample through the validator
Two examples show the validator's decision before the code. Each example gives one sample to a new validator, one that has seen no sample yet, at the ROS time \(t\) = 100 s. This is the ROS time of most tests, their constant NOW. Both samples are in the frame imu_link, with finite values and positive variances, as the firmware sends them. They differ only in their stamp \(t_s\):
- Sample B, stamped \(t_s\) = 99.900000 s: its age \(t - t_s\) is 0.100000 s, exactly
max_age. The validator accepts B, and the node forwards the sample. - Sample A, stamped \(t_s\) = 99.899999 s: its age \(t - t_s\) is 0.100001 s, 1 µs more than
max_age. The validator rejects A asTOO_OLD, and the node drops the sample.
The figure shows the decision's steps in the order of its arrows. A sample that meets a step's condition goes on to the next step. A sample that doesn't takes the arrow "no" to its drop reason, in a dashed red box. The numbers 1 to 5 are those of the five checks on the Localization page.
drop_reason_or_record() about one sample, along the arrows from the box of the arriving sample. A check lets the sample go on to the next box only if its condition holds. Otherwise the arrow "no" leads to the drop reason, and the decision ends with this reason. The clock step drops no sample: the decision always goes on after it.Why B passes and A fails, how to read the figure, and the two ends of the decision
Check 4 lets a sample through that is at most max_age, 0.1 s, old: \(t - t_s \le \texttt{max\_age}\). B is exactly that old and passes. Check 5 has no stamp of an earlier valid sample to compare B's stamp with, so B passes check 5 too, and the validator accepts B. A is 1 µs older than B and fails check 4. The same two samples appear in the test of Check 4: the two time limits, which gives both to one validator.
The figure shows every step of the decision, in the order of its arrows. On a wide screen, the steps run from left to right in two rows. On a narrow screen, they run from top to bottom. Each step is a box with a condition. A sample that meets the condition follows the arrow to the next step. A sample that doesn't meet the condition takes the arrow "no" to the reason for which the validator rejects the sample, in a dashed red box. The second step drops no sample: after the ROS time went back, the validator sets its attributes last_valid_stamp and last_valid_arrival_time to None. These two attributes hold the stamp and the arrival time of the last valid sample. Then the decision goes on, as a clock that goes back explains. The numbers 1 to 5 are those of the five checks on the Localization page.
The decision has two ends:
- Accepted:
drop_reason_or_record()stores the sample's stamp \(t_s\) in the validator's attributelast_valid_stampand its arrival time \(t\) inlast_valid_arrival_time, and returnsNone. The node counts the sample as forwarded and publishes the sample for the EKF. The tests check this end withassert_accepted(). - Rejected:
drop_reason_or_record()returns the reason. The validator stores neither the sample's stamp nor its arrival time inlast_valid_stampandlast_valid_arrival_time. At a ROS time other than 0, the clock check has already stored the call's ROS time inlast_checked_ros_time. The node counts the sample under this reason, which the diagnostics report, and passes a warning torospy.logwarn_throttle()with the periodWARNING_PERIOD, 5 s. The tests check this end withassert_rejected().
Run the tests
Open a shell in which the workspace is sourced and start the file with Python's unittest from its folder:
1 2 | |
A pass prints one line for each test, with ok at its end, and ends with Ran 30 tests and OK. catkin runs the file with the package's other tests, and CI runs it on every pull request, as Testing and CI describes.
How the package registers its three EKF tests
catkin runs the file with nose, a test runner for Python, as part of the package's tests. The excerpt shows how diffbot_control registers its three EKF tests, from CMakeLists.txt, lines 226–234. catkin_add_nosetests() registers this file. add_rostest() registers the two scenarios of the stepped-clock test, one .test file each:
| diffbot_control/CMakeLists.txt: the package's tests | |
|---|---|
226 227 228 229 230 231 232 233 234 | |
The output of a run
This is the whole output of a run at the commit 3882a19 of diffbot#220, whose files are those of its merge commit 7d428c2, in the dev container's image with ROS Noetic and Python 3.8.10. The run started python3 -m unittest -v test_imu_validator in the folder diffbot_control/test, the folder that roscd changes to. With the option -v, unittest prints one line for each test: the test's name, its class in parentheses, and ok for a test that passed. unittest runs the classes, and the tests in each class, in the alphabetical order of their names, so the output's order differs from the file's. ValidatorTestCase has no tests, so the output has no line for this class. The last lines give the number of tests and the time that they took, and OK when every test passed:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 | |
What a pass proves and what a failure means
A pass means that, for the tests' samples and times, the validator decides as the five checks describe, and that its diagnostics report the expected silence, level and counts.
A failure names the test and the assertion, with the value that came back. The test's name says which decision went wrong, and its section below shows the test. For example, test_validator_accepts_a_sample_max_age_old_and_rejects_an_older_one expects that the validator accepts a sample exactly 0.1 s old and rejects one 1 µs older as TOO_OLD. Its failure shows which of the two decisions differed and the value that came back. The tests give the validator their own limits, so a changed launch parameter max_age doesn't make them fail.
What the tests don't show
- The node: no test creates an
ImuValidatorNode, with its topics, parameters and lock. - The real clock and the real IMU: whether
max_ageandmax_futurefit the robot's USB link is a measurement on the robot. - Values that the validator doesn't check: a finite but implausible value passes by design.
The details
- The node: the class
ImuValidatorNodereads the parameters, subscribes to the topicimu/data_raw, publishes on the topicimu/data_validatedand on the topic/diagnostics, and reads the ROS time in a lock around each sample. The unit tests don't create anImuValidatorNode: they call the methods ofImuValidatordirectly. The stepped-clock test runs the node with the EKF and checks its counts on the topic/diagnostics. - The real clock and the real IMU: every time in these tests is a number that the test chose. Whether
max_ageandmax_futurefit the delays of the robot's USB link is a measurement on the robot, as Limits says. The issue IMU validator: measure the samples' age on the robot, then set max_age and max_future (diffbot#219) measures it. - The fields that the validator doesn't check: the overview lists them. No test sends a sample with an implausible but finite value, because the validator accepts such a sample by design. The EKF doesn't drop such a value either. The first measurement initializes the filter. For each later measurement, robot_localization's EKF computes the difference between the measurement and the filter's prediction and corrects the estimate with this difference, unless a rejection threshold finds the difference too large (
ekf.cpp, lines 164–189), as the same section of the overview explains. The IMU's threshold for the angular velocity,imu0_twist_rejection_threshold, defaults to the largestdouble(ros_filter.cpp, line 1418), andekf.yamlsets none. So a large but finite yaw rate in a valid sample moves the estimate.
The test classes
The file holds the 30 tests in four test classes, one for each part of imu_validator.py that works without ROS communication:
| Test class | Tests | What it calls | What it checks |
|---|---|---|---|
TestImuValidator |
12 | ImuValidator.drop_reason_or_record() |
Which samples the validator accepts, and the reason for each sample that it rejects |
TestSecondsSinceLastValidSample |
9 | ImuValidator.seconds_since_last_valid_sample() |
The time since the last valid sample, which the diagnostics report, also when the clock goes back |
TestHealth |
5 | health() |
The level of the diagnostics status: OK, warning or error |
TestSampleCounts |
4 | SampleCounts |
The counts of forwarded and dropped samples in a diagnostics report |
A fifth class, ValidatorTestCase, holds no tests. It gives the first two test classes a new validator for each test and two assertions about the validator's decision on a sample.
What the tests call
Most tests call the class ImuValidator, which holds the validator's checks without the ROS node around them. The sections below explain the class's methods that the tests call or that run inside these calls, and the reasons to drop a sample that the class returns, with the code in closed boxes. The five checks on the Localization page explain the checks themselves.
Where the ROS time comes from
ImuValidator never reads the ROS clock. Its methods get the ROS time \(t\) as their argument now: from the node on the robot and in the simulation, and in the tests a time that the test chooses, most often NOW, 100 s.
The node's callback, and what the ROS clock returns
ImuValidator never reads the ROS clock itself. Its methods drop_reason_or_record() and seconds_since_last_valid_sample() get the ROS time \(t\) as their argument now. The caller decides which time that is:
- On the robot and in the simulation: the node's class
ImuValidatorNodecalls the two methods. For each sample, rospy calls the node's callbackon_sample(), which reads the ROS time withrospy.Time.now()and passes it todrop_reason_or_record()asnow. For each diagnostics report, the node's methodtake_snapshot()reads the ROS time in the same way and passes it toseconds_since_last_valid_sample(). - In the tests: each test passes a ROS time that it chooses, most often
NOW, 100 s. So a test can put any ROS time next to any stamp \(t_s\), and needs no clock and no waiting.
The excerpt shows on_sample(), from imu_validator.py, lines 383–398. The callback reads the ROS time with rospy.Time.now() into now and passes it with the sample to drop_reason_or_record():
| imu_validator.py: the node reads the ROS time and passes it on | |
|---|---|
383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 | |
on_sample() stores the method's result in the variable drop_reason. The result is either None or a reason to drop the sample:
None: Python's value for "nothing", here "no reason to drop the sample". The node counts the sample as forwarded and publishes the sample on the topicimu/data_validated.- A reason: a member of
DropReason. The node counts the sample by this reason withcount_dropped(). After the lock, the callback passes a warning with the reason's text torospy.logwarn_throttle()with the periodWARNING_PERIOD, 5 s, which leaves out repeated warnings within that period.
The lock self.lock around both calls keeps the diagnostics timer, which runs in another thread, from reading the validator in the middle of a sample.
What rospy.Time.now() returns depends on the ROS parameter /use_sim_time, as the ROS wiki's page Clock explains:
- On the robot: none of the launch files that start the robot sets
/use_sim_time. Which launch file starts what on the Localization page shows these files. Without the parameter,rospy.Time.now()returns the system clock of the robot's single-board computer (SBC), in seconds since 1 January 1970. This clock doesn't read 0. - In the simulation: gazebo_ros's
empty_world.launch, whichdiffbot_view.launchincludes, sets/use_sim_timeto true. Gazebo publishes the simulation's time on the topic/clock, androspy.Time.now()returns the time of the last message there. Until the node has received the first message on/clock,rospy.Time.now()returns 0. Before the checks: no clock shows what the validator does then.
The stepped-clock test sets /use_sim_time too, and publishes the messages on /clock itself.
drop_reason_or_record(): the decision about a sample
drop_reason_or_record(message, now) decides about one sample, stamped \(t_s\), that arrives at the ROS time \(t\), now. The method returns the first reason to drop the sample, or records the sample's stamp and arrival time and returns None.
The two arguments, and what the name says
drop_reason_or_record(message, now) decides about one sample. The node calls the method once for each sample, and the tests call the method through two assertions. Its two arguments are:
message: the sample, asensor_msgs/Imumessage. The sample carries its own time, the stamp \(t_s\) inheader.stamp, which the sender writes into the message. On the robot, the Teensy's firmware stamps each sample with rosserial's ROS time when it starts to read the BNO055. That time estimates when the BNO055 measured, because the sensor doesn't report that time, as the firmware's IMU publisher explains. In the simulation, Gazebo's IMU plugin stamps each sample with the simulation time of the measurement. The header also carries the frame of the values,header.frame_id. The messages and their fields on the Localization page lists the sample's fields.now: the ROS time \(t\) at which the sample arrives at the validator, arospy.Time. The sample's way from the sender to the node takes time, so \(t\) is usually a little later than \(t_s\). On the robot, rosserial synchronizes the Teensy's clock only approximately with the SBC's clock. The remaining offset can put \(t_s\) a little after \(t\). Check 4 allows a stamp up tomax_future, 0.02 s, ahead.
The validator checks both the frame and the stamp: check 1 the frame, check 4 the stamp \(t_s\) against the ROS time \(t\), and check 5 the stamp \(t_s\) against the stamp of the last valid sample.
The name lists the method's two outcomes in their order: the caller gets the drop reason, the first reason to drop the sample, or, when the sample has no drop reason, the method records the sample's two times and returns None. To record means here that the validator stores the sample's stamp in its attribute last_valid_stamp and the ROS time of the sample's arrival in last_valid_arrival_time, for its next decisions. The validator doesn't keep the message or write it to a file. The return type in the method's first line, Optional[DropReason], says the same: the result is a member of DropReason or None. So None means "no reason to drop": the sample is valid, and the node forwards the sample.
The excerpt shows the method, from imu_validator.py, lines 258–275:
| imu_validator.py: drop_reason_or_record() | |
|---|---|
258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 | |
The method runs two steps, each in a method of its own: first drop_reason(), then, only when the reason is None, record_valid_sample_times().
Step 1: drop_reason(), the decision
drop_reason() returns the first reason to drop the sample, or None when the sample passes every check: first the clock, then the clock check, then the six check functions in their order. A sample that would fail two checks gets the reason of the earlier one.
drop_reason() and the order of the checks
drop_reason() returns the first reason to drop the sample, or None when the sample passes every check. The excerpt shows the method, from imu_validator.py, lines 277–295:
| imu_validator.py: drop_reason() | |
|---|---|
277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 | |
drop_reason() works in three steps:
- No clock: if a sample arrives at the ROS time 0 s,
drop_reason()returnsDropReason.NO_CLOCKat once. The validator reads 0 s as "no clock yet", as the comment above this return says, so the method skips step 2, the clock check. The clock check compares the current ROS time withlast_checked_ros_time, the ROS time of the last clock check. At startup, before the first message on/clock, a new validator has no earlier check. Now suppose that the validator last checked the clock at 100 s and a clock publisher sends 0 s. A sample that arrives at 0 s leaves the validator's attributes as they are: 100 s inlast_checked_ros_time, and the last valid sample's stamp and arrival time inlast_valid_stampandlast_valid_arrival_time. If the next check reads 0.02 s, the check setslast_valid_stampandlast_valid_arrival_timetoNone, because 0.02 s is earlier than 100 s. That next check can come from a sample or from a diagnostics report. A test in Before the checks: no clock and two tests of the silence check that a sample at 0 s changes none of the three attributes. - A clock that went back:
clear_last_valid_sample_times_if_clock_went_back(now)clearslast_valid_stampandlast_valid_arrival_timeifnowis earlier thanlast_checked_ros_time. The method then storesnowinlast_checked_ros_timefor the next clock check. - The checks: the loop calls the checks in the tuple
sample_checks, one after the other. The first check that returns a reason ends the loop, and the method returns that reason. The later checks don't run. When every check has returnedNone, the method returnsNone.
The constructor __init__() fills sample_checks in the order in which the checks run. The excerpt shows the tuple, from imu_validator.py, lines 222–230:
| imu_validator.py: the checks in their order | |
|---|---|
222 223 224 225 226 227 228 229 230 | |
The six functions are the five checks of the Localization page. Check 4 has two functions, one for each time limit: check_not_too_far_ahead() and check_not_too_old(). A sample that would fail two checks gets the reason of the earlier one, as sample A in Check 4: the two time limits shows.
Step 2: record_valid_sample_times(), a valid sample
Only for a valid sample, record_valid_sample_times() stores the sample's stamp in last_valid_stamp and its arrival time in last_valid_arrival_time. So the stamp of a dropped sample never becomes the last valid stamp.
record_valid_sample_times(), and what a call changes
Only when drop_reason() has returned None, record_valid_sample_times() stores two times of the sample in attributes of the validator: the sample's stamp in last_valid_stamp and its arrival time in last_valid_arrival_time. The excerpt shows the method, from imu_validator.py, lines 297–303:
| imu_validator.py: record_valid_sample_times() | |
|---|---|
297 298 299 300 301 302 303 | |
record_valid_sample_times() stores the two times: the sample's stamp \(t_s\) in last_valid_stamp, the stamp \(t_\text{last}\) that check 5 compares the next stamps with, and the ROS time of its arrival, now, in last_valid_arrival_time, from which seconds_since_last_valid_sample() counts the silence. Each valid sample replaces the values of the two attributes, so the validator keeps no history of samples.
What a call of drop_reason_or_record() changes in the validator's attributes depends on how the call ends. The table lists the three cases:
| How the call ends | last_valid_stamp and last_valid_arrival_time |
last_checked_ros_time, which the next clock check compares with |
|---|---|---|
| The sample is valid | replaced by this sample's stamp and now |
now |
| A check drops the sample | unchanged, or set to None when now shows that the clock went back |
now |
The ROS time is 0 s (NO_CLOCK) |
unchanged | unchanged |
So the stamp of a dropped sample never becomes \(t_\text{last}\). The node counts every dropped sample by its reason, NO_CLOCK included, and passes a warning to rospy.logwarn_throttle() with the period WARNING_PERIOD, 5 s.
Why one method runs both steps
The node and the tests both call drop_reason_or_record(). So neither can record the times of a dropped sample or forget those of a valid one.
What each of the two mistakes would do
Two places in the code call drop_reason_or_record(): the node's callback on_sample(), once for each sample that arrives, and the tests, through assert_accepted() and assert_rejected(). Neither calls drop_reason() and record_valid_sample_times() itself. So neither can get the two steps wrong in one of two ways:
- Recording a dropped sample's times: check 5 would compare later stamps with the stamp of a sample that the checks dropped. If a caller recorded the times of a sample from the future, otherwise valid samples with stamps no later than that stored stamp would be dropped as
NOT_NEWER. - Not recording a valid sample's times: check 5 would still let the same stamp through, and the silence would keep counting from the arrival time that was recorded last. If no arrival time had been recorded, the silence would stay
None. Such omissions could make the diagnostics report an error while valid samples arrive.
The tests therefore run the same two steps as the node.
clear_last_valid_sample_times_if_clock_went_back(): a clock that goes back
When a call's ROS time is earlier than last_checked_ros_time, the ROS time of the last clock check, the clock went back. The method then sets last_valid_stamp and last_valid_arrival_time to None, so the order check and the silence start over.
The clock check, its two helpers, and a sample after the jump
The ROS time can jump back. In the simulation, a call of the service /gazebo/reset_simulation starts Gazebo's clock again near 0 s. On the robot, a correction of the SBC's clock can set the time back. Without a reaction, check 5 would still compare new stamps with the stamp \(t_\text{last}\) of the last valid sample from before the jump. Samples that pass checks 1 to 4 would then be dropped as NOT_NEWER while their stamps are no later than \(t_\text{last}\). For example, suppose the last valid stamp is 100 s and the simulation restarts near 0 s. If each new sample's stamp equals its arrival time, those samples would be dropped until their stamps exceed 100 s, about 100 s of simulation time later.
When the clock went back, the method sets the validator's attributes last_valid_stamp and last_valid_arrival_time to None. These two attributes hold the stamp of the last valid sample and the ROS time of its arrival, the last valid sample's times of the method's name. The validator stores no sample, only these two times. The validator also stores the ROS time of the last clock check in its attribute last_checked_ros_time. The excerpt shows the method and its two helpers, from imu_validator.py, lines 232–256:
| imu_validator.py: clear_last_valid_sample_times_if_clock_went_back() and its two helpers | |
|---|---|
232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 | |
clock_went_back(now): returns whethernowis earlier thanlast_checked_ros_time. Before the first clock check,last_checked_ros_timeisNone, and the answer isFalse.clear_last_valid_sample_times(): sets the attributeslast_valid_stampandlast_valid_arrival_timetoNone, as before the first valid sample.clear_last_valid_sample_times()undoes whatrecord_valid_sample_times()stores.clear_last_valid_sample_times_if_clock_went_back(now): callsclear_last_valid_sample_times()only whenclock_went_back(now)is true. Then the method storesnowinlast_checked_ros_timefor the next clock check, whether the clock went back or not.
After a jump back, the validator starts over as if it had seen no valid sample yet:
- Check 5: with no stamp \(t_\text{last}\) to compare with, check 5 lets the next sample through. If checks 1 to 4 pass too, the sample is valid, and its stamp becomes the new \(t_\text{last}\).
- The silence:
seconds_since_last_valid_sample()returnsNoneuntil a sample is valid again. A diagnostics report in that time shows the error "no IMU sample forwarded yet, or since the clock went back". At 50 Hz, samples are spaced 20 ms apart. The silence becomes a number again only when a sample passes all five checks.
The table follows two samples through a new validator, one that has seen no sample yet, with the values of the test test_validator_accepts_a_stamp_once_after_the_clock_goes_back in A clock that goes back:
- The first sample: is stamped 100 s and arrives at the ROS time 100 s. Both values are the test's
NOW. - Sample J: arrives after the clock went back, as after a call of
/gazebo/reset_simulation. J is stamped 0.02 s and arrives at the ROS time 0.02 s. Both values are the test's constantRESTART_NEAR_ZERO.
Each column of the table is one sample. The rows follow the sample's call of drop_reason_or_record() from top to bottom. The clock check stores its ROS time in the validator's attribute last_checked_ros_time. The row "Result" gives the values of the attributes last_valid_stamp and last_valid_arrival_time after the call:
| Step of the call | The first sample | Sample J |
|---|---|---|
| Stamp \(t_s\) | 100 s | 0.02 s |
| ROS time \(t\) at arrival | 100 s | 0.02 s |
| Clock check: \(t\) against the ROS time of the last clock check | No clock check yet, so nothing to compare with | 100 s. 0.02 s is earlier, so the clock went back: the check sets last_valid_stamp and last_valid_arrival_time to None |
last_checked_ros_time after the clock check |
100 s | 0.02 s |
| Check 5: \(t_s\) against the last valid stamp | No stamp to compare with, so the check passes | No stamp to compare with, so the check passes |
| Result | Valid. last_valid_stamp: 100 s. last_valid_arrival_time: 100 s |
Valid. last_valid_stamp: 0.02 s. last_valid_arrival_time: 0.02 s |
Without the clock check, last_valid_stamp would still be 100 s when J arrives. Check 5 would drop J as NOT_NEWER, because J's stamp of 0.02 s is not later than 100 s.
Two methods call clear_last_valid_sample_times_if_clock_went_back() with their own now: drop_reason() for each sample that arrives at a ROS time other than 0, and seconds_since_last_valid_sample() for each diagnostics report. So the validator notices a jump back also while no sample arrives. The tests never call the method directly. They see its effect in A clock that goes back and in the tests of the silence.
seconds_since_last_valid_sample(): the silence
seconds_since_last_valid_sample(now) checks the clock first. Then it returns the silence \(t - t_\text{valid}\) in s, the ROS time since the last valid sample arrived, or None when no sample was valid since the clock last went back. The validator's diagnostics report this silence.
seconds_since_last_valid_sample()
seconds_since_last_valid_sample(now) returns how long no sample has been valid at the ROS time now. The validator's diagnostics report this silence. The excerpt shows the method, from imu_validator.py, lines 342–356:
| imu_validator.py: seconds_since_last_valid_sample() | |
|---|---|
342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 | |
The method first calls clear_last_valid_sample_times_if_clock_went_back(), so the validator notices a clock that went back also while no sample arrives. Then the method returns one of two things:
None: no sample was valid since the clock last went back, or none at all.- The silence in s, a
float: the ROS time \(t\) minus \(t_\text{valid}\), the ROS timelast_valid_arrival_timeat which the last valid sample arrived: \(t - t_\text{valid}\). The silence counts from the sample's arrival, not from its stamp.
DropReason: the reasons to drop a sample
DropReason holds one member for each of the seven reasons to drop a sample, each with the text that the log and the diagnostics show, such as TOO_OLD with too old.
The seven members, and DropReason in the code
The table lists the seven members with the check that returns each:
| Member | Returned by | Text in the log and on /diagnostics |
|---|---|---|
NO_CLOCK |
drop_reason() itself, before the checks, at the ROS time 0 |
no ROS clock |
WRONG_FRAME |
Check 1, check_frame() |
wrong frame |
NOT_FINITE |
Check 2, check_finite_values() |
not finite |
INVALID_ANGULAR_VELOCITY_COVARIANCE |
Check 3, check_angular_velocity_covariance() |
invalid angular velocity covariance |
FROM_THE_FUTURE |
Check 4, check_not_too_far_ahead() |
from the future |
TOO_OLD |
Check 4, check_not_too_old() |
too old |
NOT_NEWER |
Check 5, check_stamp_later_than_last_valid() |
not newer than the last one |
DropReason is an enumeration, a fixed set of named members, one for each reason to drop a sample. The excerpt shows the class, from imu_validator.py, lines 105–118:
| imu_validator.py: DropReason | |
|---|---|
105 106 107 108 109 110 111 112 113 114 115 116 117 118 | |
Each member's value is a text. The node writes the text into its warning in the log, as on_sample() shows. The diagnostics count the dropped samples by reason, each under a key that starts with Dropped, and ends with the text, as Dropped, too old in the example of the validator's diagnostics. The table lists the seven members with the check that returns each:
The tests import the module imu_validator and not its names. So a test writes a member with the module's name in front, as imu_validator.DropReason.TOO_OLD.
The constants and the helpers
The tests share constants, such as NOW, 100 s, the ROS time of most checks, and two helpers: make_imu_sample() builds a sample like the firmware's that is valid at NOW for a new validator, and seconds() turns a number of seconds into a rospy.Duration.
The constants of test_imu_validator.py
The excerpt shows all constants of the file, from test_imu_validator.py, lines 23–65. NOW is the ROS time \(t\) of most checks, 100 s. MAX_AGE and MAX_FUTURE are the two time limits that ekf.launch sets on the robot and in the simulation. JUST_TOO_OLD and JUST_TOO_FAR_AHEAD lie ONE_MICROSECOND, 1 µs, beyond these limits. The tests that use the other constants explain them in their sections, and the comment above each constant says which condition its value meets. ONE_NANOSECOND is a rospy.Duration of exactly 1 ns, built from whole nanoseconds and not with seconds(). seconds() passes a number of seconds to rospy.Duration.from_sec(), which multiplies the number by 10⁹ as a floating-point number and cuts off the fraction of a nanosecond. So seconds(3.3) is 3.299999999 s.
| test_imu_validator.py: the constants | |
|---|---|
23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 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 | |
make_imu_sample() and seconds()
Every test that gives the validator a sample builds the sample with the helper make_imu_sample(). The excerpt shows make_imu_sample() and the helper seconds(), from test_imu_validator.py, lines 68–89:
| test_imu_validator.py: make_imu_sample() and seconds() | |
|---|---|
68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 | |
make_imu_sample(): returns asensor_msgs/Imumessage like the firmware's. With the defaults of its three arguments, the sample is valid at the ROS timeNOWfor a new validator, one that has seen no sample yet. If the last valid sample has the same stamp, check 5 drops the new sample. With the defaults, the sample's stamp \(t_s\) isNOW, its frame isimu_linkand its yaw rate \(\omega_z\) isYAW_RATE, 0.5 rad/s. The accelerometer's reading on the upward axis, \(f_z\), isGRAVITY, 9.81 m/s², as for an IMU at rest.orientation_covariance[0]is −1, which marks the orientation as unavailable, as in the firmware's message.- The variances: each variance on the diagonal of the angular velocity's covariance \(\boldsymbol\Sigma_{\boldsymbol\omega}\), such as \(\sigma^2_{\omega_z}\), is
ANGULAR_VELOCITY_VARIANCE, 4 · 10⁻⁴ (rad/s)². Each variance on the diagonal oflinear_acceleration_covarianceisLINEAR_ACCELERATION_VARIANCE, 10⁻² (m/s²)². Both have the size of the firmware's variances. - Valid and invalid samples: the tests override the defaults of
stamp,frame_idoryaw_rate, or change a field of the returned message, to build the samples that they need. A later stamp gives the next valid sample, for example. An invalid sample differs from the valid one in one property, so that the sample aims at one check. A sample can fail a later check too, as sample A of the time limits does. seconds(): turns a number of seconds into arospy.Duration, which a test adds toNOWor subtracts from it.
ValidatorTestCase: a new validator and two assertions
TestImuValidator and TestSecondsSinceLastValidSample derive from ValidatorTestCase. Its setUp() gives each test a new validator. Its assertions assert_accepted() and assert_rejected() call drop_reason_or_record() once, as the node does for each sample, and check that the validator accepts the sample or rejects it with the expected reason.
ValidatorTestCase in the code
TestImuValidator and TestSecondsSinceLastValidSample derive from the class ValidatorTestCase, which derives from unittest.TestCase. A class that derives from another class inherits the other class's methods. So the tests of both classes can use the three methods of ValidatorTestCase: setUp() and the two assertions assert_accepted() and assert_rejected(). ValidatorTestCase itself has no tests. The excerpt shows the class, from test_imu_validator.py, lines 92–110:
| test_imu_validator.py: ValidatorTestCase | |
|---|---|
92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 | |
setUp(): unittest callssetUp()before each test method. So each test of a class that derives fromValidatorTestCasestarts with a newImuValidatorinself.validator, one that has seen no sample yet. No test depends on the samples of another test.FRAME,MAX_AGEandMAX_FUTUREare the values thatekf.launchgives the validator's node.assert_accepted(message, now): callsdrop_reason_or_record(message, now)once, the same method that the node calls once for each sample, and checks the result withassertIsNone(). A result ofNonemeans that the sample has no drop reason: the validator accepts the sample and stores the sample's stamp inlast_valid_stampand its arrival time inlast_valid_arrival_time. For any other result,assertIsNone()stops the test, and unittest reports the test as failed, with the drop reason that came back instead ofNone.assert_rejected(reason, message, now): callsdrop_reason_or_record(message, now)once too, and compares the result with the expectedreasonbyassertEqual().assertEqual()compares its first two arguments and fails the test when they differ. The tests of this file giveassertEqual()the expected value first and the result second, andassert_rejected()does the same. So the assertion holds when the validator rejects the sample with the expected reason. The validator doesn't store a rejected sample's stamp or arrival time inlast_valid_stampandlast_valid_arrival_time. Only the clock check, which runs before the checks at any ROS time other than 0, stores the call's ROS time inlast_checked_ros_time.msg: the optional last argument of both assertions. The typeOptional[str]says thatmsgis a text orNone, and= NonemakesNonethe default. When the assertion fails, unittest adds the text to its report of the failure. The tests of check 1, check 2 and check 3 run a loop over several cases and give each assertion a text that names the case, so that a failure says which case the validator decided wrongly.
assertIsNone() and assertEqual() are two of the assert methods that every test class inherits from unittest.TestCase, which is why the two assertions call these methods on self. The tests call assert_accepted() and assert_rejected() on self in the same way.
Accepted and rejected are the validator's decisions, and the tests' names say "accepts" and "rejects". The node forwards an accepted sample and drops a rejected one. The tests themselves publish nothing, as What the tests don't show says.
TestImuValidator: which samples are valid
The class tests drop_reason_or_record(), one reason to drop a sample at a time, in the order of the checks. Each test starts with a new validator.
The class in the code
The class tests drop_reason_or_record(), one reason to drop a sample at a time. The excerpt shows the class's first two lines, from test_imu_validator.py, lines 113–114. The name in parentheses, ValidatorTestCase, is the class that TestImuValidator derives from. So each test starts with a new validator and checks each decision of the validator with assert_accepted() or assert_rejected():
| test_imu_validator.py: the class derives from ValidatorTestCase | |
|---|---|
113 114 | |
The sections below follow the order of the checks, with the numbers and names of the five checks on the Localization page.
A valid sample
test_validator_accepts_valid_samples is the class's baseline. The validator accepts a sample stamped NOW at NOW, and then the IMU's next sample at 50 Hz, stamped and arriving 0.02 s later.
The test
test_validator_accepts_valid_samples is the baseline of the class. The excerpt shows the test, from test_imu_validator.py, lines 116–118:
| test_imu_validator.py: valid samples | |
|---|---|
116 117 118 | |
The test's first line holds two calls, one inside the other. Python runs the inner call first:
- The inner call:
make_imu_sample()returns a valid sample, stampedNOW. - The outer call:
self.assert_accepted(make_imu_sample(), NOW)gets two arguments. The first is the sample from the inner call. The second is the ROS time \(t\) of the sample's arrival,NOW, 100 s. The assertionassert_accepted()callsdrop_reason_or_record()with both and checks that the method returnsNone, no drop reason. Otherwise the test stops there, and unittest reports the test as failed, with the drop reason that came back instead ofNone.
So the first line checks that the validator accepts the sample. In the node, on_sample() forwards an accepted sample.
The second line checks a second sample in the same way. The sample is stamped MAX_FUTURE, 0.02 s, later than the first, and arrives at a ROS time that is 0.02 s later too. This sample is the next one of the IMU at 50 Hz. The validator accepts the sample, because its stamp is later than the first sample's.
Before the checks: no clock
At the ROS time 0, the validator has no clock yet. It rejects every sample with NO_CLOCK before the checks, because a stamp can't be judged against this time. A second test checks that such a sample leaves last_valid_stamp as it is.
The two tests
test_validator_rejects_a_sample_at_ros_time_zero gives the validator a valid sample at the ROS time 0, the test's constant ROS_TIME_ZERO, rospy.Time(0). The excerpt shows this test and the next one, from test_imu_validator.py, lines 120–134:
| test_imu_validator.py: no clock, and what a sample at 0 s leaves | |
|---|---|
120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 | |
assert_rejected() gets the expected reason first, here imu_validator.DropReason.NO_CLOCK, then the sample and the ROS time. So the first test checks that the validator rejects even a valid sample at the ROS time 0, with the reason NO_CLOCK.
The validator treats the ROS time 0 as an unavailable clock. At startup with simulation time, rospy.Time.now() returns 0 until the node has received the first message on the topic /clock, as Where the ROS time comes from explains. A stamp can't be judged against this time. At \(t = 0\), check 4 would allow only the stamps between −0.1 s and 0.02 s:
- A sample from Gazebo: a valid sample stamped, for example, 0.5 s would be rejected as from the future. The log and the diagnostics would blame the sample's stamp, although the node's clock is missing.
- A sample without a stamp: a message whose stamp was never set has the stamp 0 s, and would pass check 4.
So the validator rejects every sample at the ROS time 0 before the checks, with its own reason NO_CLOCK, which drop_reason() returns. On the robot, the ROS time is the SBC's system clock, which doesn't read 0. So the robot's validator doesn't reject samples for this reason.
test_validator_rejects_the_same_stamp_again_after_a_sample_at_ros_time_zero checks that a sample at the ROS time 0 leaves last_valid_stamp as it is, because drop_reason() returns NO_CLOCK before the clock check. The test gives one validator three samples:
- The 1st call, at the ROS time
NOW, 100 s: a sample stampedNOWis valid. The validator stores 100 s in its attributeslast_valid_stampandlast_checked_ros_time. - The 2nd call, at
ROS_TIME_ZERO, 0 s: a sample stampedSHORTLY_BEFOREbeforeNOW, 99.99 s. The validator returnsNO_CLOCKbefore the clock check. Both attributes stay 100 s. - The 3rd call, at
NOWplusONE_MILLISECOND, 100.001 s: a sample stampedNOWagain. The clock check finds 100.001 s later thanlast_checked_ros_time, 100 s, and stores 100.001 s.last_valid_stampis still 100 s, so check 5 rejects the sample asNOT_NEWER.
The third call's own clock check finds no clock that went back, because its ROS time is not earlier than NOW. So only the second call could have changed last_valid_stamp. A validator that checked the clock before it returned NO_CLOCK would take 0 s for a clock that went back and set its attributes last_valid_stamp and last_valid_arrival_time to None. A validator that stored the second sample's stamp would compare the third stamp with 99.99 s. Either would accept the third sample, and the test fails for both. Two tests of the silence check the other two attributes.
Check 1: the frame
A sample in the frame base_link or in an empty frame is rejected as WRONG_FRAME.
The test
Check 1 compares the sample's header.frame_id with the frame that the validator expects, FRAME, imu_link. The excerpt shows the test, from test_imu_validator.py, lines 136–140:
| test_imu_validator.py: a wrong frame | |
|---|---|
136 137 138 139 140 | |
The loop runs twice, with the frame base_link and with an empty frame. In each pass, assert_rejected() expects DropReason.WRONG_FRAME. The last argument is the assertion's msg, the text that unittest adds to its report when the assertion fails. The text names the frame of the loop's current pass, so a failure says which of the two frames the validator didn't reject.
Check 2: the values
A sample with NaN, +∞ or −∞ as its yaw rate or as its acceleration is rejected as NOT_FINITE.
The test
Check 2 needs finite values in the angular velocity \(\boldsymbol\omega\), the field angular_velocity, and in the accelerometer's reading \(\mathbf f\), the field linear_acceleration. The excerpt shows the test, from test_imu_validator.py, lines 142–151:
| test_imu_validator.py: NaN and infinite values | |
|---|---|
142 143 144 145 146 147 148 149 150 151 | |
The loop runs three times, for NaN, +∞ and −∞. Each pass builds two samples. The first has the value as its yaw rate \(\omega_z\), angular_velocity.z. The second has the value as \(f_x\), linear_acceleration.x. The validator has to reject all six samples with DropReason.NOT_FINITE.
Check 3: the variances
Three samples, each with one defect in the angular velocity's covariance, are rejected as INVALID_ANGULAR_VELOCITY_COVARIANCE: an unavailable angular velocity, a zero variance of the yaw rate, and NaN off the diagonal.
The test
Check 3 looks at the covariance \(\boldsymbol\Sigma_{\boldsymbol\omega}\) of the angular velocity \(\boldsymbol\omega\), the field angular_velocity_covariance. Its nine values have to be finite, and its three variances on the diagonal, \(\sigma^2_{\omega_x}\), \(\sigma^2_{\omega_y}\) and \(\sigma^2_{\omega_z}\), have to be positive. The variances on the Localization page shows how the message stores the 3 × 3 matrix as nine numbers, row by row. The excerpt shows the test, from test_imu_validator.py, lines 153–164:
| test_imu_validator.py: three invalid covariances | |
|---|---|
153 154 155 156 157 158 159 160 161 162 163 164 | |
The test builds three samples, each with one defect in \(\boldsymbol\Sigma_{\boldsymbol\omega}\), and expects DropReason.INVALID_ANGULAR_VELOCITY_COVARIANCE for each. The text in each pair of cases is the msg of a failure:
unavailable: the first element is −1 and the others are 0. This is how asensor_msgs/Imumessage says that it carries no angular velocity. Check 3 reads the three variances \(\sigma^2_{\omega_x}\), \(\sigma^2_{\omega_y}\) and \(\sigma^2_{\omega_z}\) as −1, 0 and 0 (rad/s)², so none of them is positive.zero_variance: the variance of the yaw rate \(\sigma^2_{\omega_z}\), the element with the indexYAW_RATE_VARIANCE_INDEX, 8, is 0 (rad/s)².nan_off_the_diagonal: the covariance \(\sigma_{\omega_x \omega_y}\), the element with the indexOFF_DIAGONAL_INDEX, 1, is NaN. This element is not a variance, so the test shows that the check looks at all nine values.
Check 4: the two time limits
Check 4 compares the stamp \(t_s\) with the ROS time \(t\): \(t - \texttt{max\_age} \le t_s \le t + \texttt{max\_future}\). Two tests probe each limit from both sides. The excerpt shows both tests, from test_imu_validator.py, lines 166–174:
| test_imu_validator.py: at the limits and 1 µs beyond | |
|---|---|
166 167 168 169 170 171 172 173 174 | |
The figure shows the samples of these two tests on a time axis. The sections on check 5 and on a clock that goes back have figures of the same kind:
- A✕ dropped: stamped t − 0.100001 s, 1 µs older than MAX_AGE allows
- B✓ forwarded: stamped t − 0.1 s, exactly MAX_AGE old
- C✓ forwarded: stamped t + 0.02 s, exactly MAX_FUTURE ahead
- D✕ dropped: stamped t + 0.020001 s, 1 µs further ahead than MAX_FUTURE allows
The validator has to accept B and C, so each limit itself is still valid, and reject A as TOO_OLD and D as FROM_THE_FUTURE.
How to read the three time-axis figures
The figure above shows the samples of these two tests on a time axis. The sections on check 5 and on a clock that goes back have figures of the same kind. Read the three figures as follows:
- The axis: time runs to the right. One tick is 0.02 s, the IMU's sample period at 50 Hz. Each axis has its own scale.
- A mark: stands at the stamp \(t_s\) of its sample. Its letter is the sample's name in the text. A solid green mark is a sample that the validator has to accept, a dashed red mark one that the validator has to reject.
- The bands: a stamp in a green band passes the check. A stamp in a red band is rejected for the reason that the band names.
- Two slanted strokes: the axis leaves out a stretch of time there.
- The list below a figure: says for each sample what the node does with the sample, and why. "Forwarded" stands for a sample that the validator accepts, "dropped" for one that the validator rejects.
Two samples that lie only 1 µs or 1 ns apart would share one spot at the figure's scale, so the figures draw them apart.
The stamps from the constants, and why A is too old and not "not newer"
The first test sends B and then A, the second test D and then C, all at the ROS time NOW. The tests build the four stamps from the constants: B is NOW - seconds(MAX_AGE), A is NOW - seconds(JUST_TOO_OLD), D is NOW + seconds(JUST_TOO_FAR_AHEAD) and C is NOW + seconds(MAX_FUTURE). The validator has to accept B and C, so each limit itself is still valid. The validator has to reject A with DropReason.TOO_OLD and D with DropReason.FROM_THE_FUTURE.
In the first test, sample A is also older than the valid sample B before it. The validator reports DropReason.TOO_OLD and not DropReason.NOT_NEWER, because the checks run in their order and check 4 comes before check 5. The second test sends the rejected sample D first. For a rejected sample at a ROS time other than 0, the validator stores only the ROS time of the call in last_checked_ros_time, for its clock check, so D doesn't change what the validator decides for C.
Check 5: the order of the stamps
Check 5 lets a stamp through only when it is later than \(t_\text{last}\), the stamp of the last valid sample. After a valid sample stamped NOW, a sample with the same stamp or an earlier one is rejected as NOT_NEWER, a sample 1 ns later is accepted, and a rejected sample from the future doesn't become \(t_\text{last}\):
- E✕ dropped: stamped 0.01 s before 100 s. The sample arrived out of order
- F✕ dropped: stamped 100 s again. The sample came twice
- G✓ forwarded: stamped 1 ns after 100 s, the smallest step of a stamp
- H✕ dropped: stamped 105 s. Check 4 drops it as too far ahead
- I✓ forwarded: sent after H, stamped 100.02 s. The validator compares the stamp with 100 s, not with the stamp of H
The two tests
Check 5 compares the stamp \(t_s\) with \(t_\text{last}\), the stamp of the last valid sample: \(t_s > t_\text{last}\). The excerpt shows the two tests of this check, from test_imu_validator.py, lines 176–194:
| test_imu_validator.py: the order check | |
|---|---|
176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 | |
test_validator_rejects_a_sample_not_later_than_the_last_valid_one sends four samples to one validator:
- A valid sample stamped
NOW. The validator storesNOWinlast_valid_stamp, as \(t_\text{last}\). - Sample F, stamped
NOWagain. The validator rejects the sample withDropReason.NOT_NEWER, because F's stamp is not later than \(t_\text{last}\). A stamp is valid only once, until the clock goes back. - Sample E, stamped
SHORTLY_BEFORE, 0.01 s, beforeNOW.SHORTLY_BEFOREis a tenth ofMAX_AGE, so check 4 lets E through, and check 5 rejects the sample with the same reason. - Sample G, stamped
ONE_NANOSECOND, 1 ns, afterNOW, the smallest step of a ROS stamp. G arrivesONE_MILLISECOND, 1 ms, afterNOW, so after its own stamp. A ROS time beforeNOWwould be a clock that went back and would clearlast_valid_stamp. The validator accepts the sample.
test_validator_accepts_a_new_sample_after_rejecting_a_future_one shows what \(t_\text{last}\) is not. After a valid sample stamped NOW, the test sends sample H, stamped FAR_AHEAD, 5 s, ahead. Check 4 rejects H with DropReason.FROM_THE_FUTURE. Then the test sends sample I, stamped 0.02 s after NOW. The validator accepts I. If the validator stored the stamp of the rejected sample H in last_valid_stamp, every sample of the next 5 s would count as not newer.
A clock that goes back
After the ROS time went back, the order check starts over, but check 4 judges the stamps against the new ROS time. The figure shows the two tests, each on its own axis at the new ROS time:
The clock goes back from 100 s to 0.02 s: check 5 starts over
- J✓ forwarded: stamped 0.02 s, the new ROS time. The stamp is far older than the last valid stamp, 100 s. When the clock went back, the validator set its attribute last_valid_stamp to None
- K✕ dropped: stamped 0.02 s again, sent 1 ms later. The stamp is not later than the stamp of J
- 100 s, the stamp of the last valid sample before the clock went back
The clock goes back from 100 s to 50 s: check 4 uses the new time
- L✕ dropped: stamped 49 s, 1 s older than the new ROS time
- M✕ dropped: stamped 100.02 s, 50.02 s ahead of the new ROS time. At the ROS time 100 s, this stamp was valid
The two tests
As clear_last_valid_sample_times_if_clock_went_back() explains, the validator sets its attributes last_valid_stamp and last_valid_arrival_time to None when a call checks the clock and the call's now is earlier than last_checked_ros_time, the ROS time of the last clock check. Every diagnostics report checks the clock, and so does every sample that arrives at a ROS time other than 0. A sample at the ROS time 0 returns NO_CLOCK before the clock check and changes nothing. After the validator has cleared last_valid_stamp and last_valid_arrival_time, the order check, check 5, starts over. The excerpt shows the two tests, from test_imu_validator.py, lines 196–213:
| test_imu_validator.py: the clock goes back | |
|---|---|
196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 | |
test_validator_accepts_a_stamp_once_after_the_clock_goes_back: the first sample is stampedNOW, 100 s, and arrives at the ROS timeNOW. The validator accepts this sample. Then the ROS time isRESTART_NEAR_ZERO, 0.02 s, as after a reset of the simulation. This test is the first axis of the figure. Sample J is stampedRESTART_NEAR_ZEROand arrives at the ROS timeRESTART_NEAR_ZERO. This stamp is far older than \(t_\text{last}\), 100 s. The validator accepts J all the same, because the order check started over. The order check works again from this sample on: the test sends the same stamp once more,ONE_MILLISECOND, 1 ms, later, and expectsDropReason.NOT_NEWER. This second sample is K.test_validator_rejects_stamps_outside_time_limits_after_the_clock_goes_back: starting over applies to the order check only. The first sample is again stampedNOW, 100 s, and arrives at the ROS timeNOW. Then the ROS time isRESTART, 50 s, and both of the next samples arrive at this ROS time. This test is the second axis of the figure. Sample M is stamped 100.02 s,NOWplusMAX_FUTURE. This stamp was valid before the clock went back and is 50.02 s ahead of the new ROS time. The validator rejects M withDropReason.FROM_THE_FUTURE. Sample L is stamped 49 s,ONE_SECOND, 1 s, beforeRESTART, and the validator rejects L withDropReason.TOO_OLD.
TestSecondsSinceLastValidSample: the silence
The validator's diagnostics report an error when no sample was forwarded for too long. The time behind this error comes from seconds_since_last_valid_sample(), which returns the silence \(t - t_\text{valid}\) in s, or None when no sample was valid since the clock last went back.
Nine tests in four groups check the silence. Each figure draws each test's calls as rows from top to bottom, above the ROS time.
How to read the silence figures
Read the drawings as follows:
- A row: one call of the test, in the order of the test's lines, from top to bottom. The list below the drawing gives each call with the same number, the test's names for its times and their values.
- A circle: a sample that the test gives the validator. A solid green circle is a sample that the validator accepts. A dashed red circle is a sample that the validator rejects, with the drop reason beside it.
- A square: a call of
seconds_since_last_valid_sample(), withNonebeside it or the silence that it returns. - The axis: the ROS time \(t\) of the calls. Each mark stands at the ROS time of its call. Two slanted strokes leave out a stretch of time, and each part of the axis between them has its own scale.
- A triangle: a sample's stamp \(t_s\), where it differs from the ROS time of the sample's arrival. A dotted line joins the triangle to the sample's mark.
- A green line:
last_valid_arrival_time, \(t_\text{valid}\). The line runs down from the valid sample that stored it. A cross ends the line above the row of the call that sets the attribute toNone. - A dashed gray line:
last_checked_ros_time, the ROS time of the last clock check. The line runs down from each call that checks the clock to the next call that checks it. A sample at the ROS time 0 doesn't check the clock, so the line passes its row. Where the dashed line and the green line stand at the same time, the dashed line lies on the green one. - An arrow to the left, "back": the call's ROS time is earlier than
last_checked_ros_time, so the clock went back. - A dimension line: the silence \(t - t_\text{valid}\) that a call of
seconds_since_last_valid_sample()returns, with the test's name for the expected value.
The class and its helper assert_silence()
The class derives from ValidatorTestCase too. So each test starts with a new validator, and the tests give the validator their samples through assert_accepted() and assert_rejected(). The class adds a helper of its own, assert_silence(), for the comparison of the silence. The excerpt shows the class's first lines and the helper, from test_imu_validator.py, lines 216–224:
| test_imu_validator.py: the class and assert_silence() | |
|---|---|
216 217 218 219 220 221 222 223 224 | |
assert_silence(expected, now) calls seconds_since_last_valid_sample(now) and compares the result with expected. If the method returns None, assert_silence() fails the test with a message that names the expected silence. Otherwise assertAlmostEqual() rounds the difference of the two numbers to seven decimal places, its default, and expects zero.
Before and after a valid sample
The first three tests show where the silence counts from. The first test asks a new validator for the silence. The second asks after one valid sample, and the third after a valid sample and a rejected one:
Before the first valid sample
- 1returns None: seconds_since_last_valid_sample() at NOW, 100 s
After a valid sample
- 1✓ accepted: a sample stamped NOW − SHORTLY_BEFORE, 99.99 s, at NOW, 100 s
- 2returns SILENCE, 3.5 s: seconds_since_last_valid_sample() at NOW + SILENCE, 103.5 s
While samples are rejected
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2✕ TOO_OLD: a sample stamped NOW + ONE_SECOND, 101 s, at NOW + SILENCE, 103.5 s
- 3returns SILENCE, 3.5 s: seconds_since_last_valid_sample() at NOW + SILENCE, 103.5 s
The three tests
The excerpt shows the three tests, from test_imu_validator.py, lines 226–242:
| test_imu_validator.py: the silence grows | |
|---|---|
226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 | |
- Before the first valid sample: the method returns
None. - After a valid sample: the valid sample is stamped
SHORTLY_BEFORE, 0.01 s, before it arrives atNOW.SILENCE, 3.5 s, after the arrival, the method returns 3.5 s and not 3.51 s, because the silence counts from the arrival and not from the stamp.SILENCEhas a fraction of a second, so a silence counted in whole seconds would fail too. - While samples are rejected: a sample that the validator rejects with
DropReason.TOO_OLDdoesn't end the silence. The rejected sample is stampedONE_SECONDafterNOW, later than the valid sample's stamp, so check 5 would let it through. The sample arrivesSILENCEafterNOW, when its stamp is 2.5 s old, more thanMAX_AGE. So check 4 rejects the sample before check 5 runs. At the same ROS time,SILENCE, 3.5 s, after the valid sample, the method returns 3.5 s.
After a sample at the ROS time 0
A sample at the ROS time 0 returns NO_CLOCK before the clock check and leaves the validator's attributes as they are. The two tests check last_valid_arrival_time and last_checked_ros_time:
The arrival time
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2✕ NO_CLOCK: a sample stamped NOW − SHORTLY_BEFORE, 99.99 s, at ROS_TIME_ZERO, 0 s
- 3returns RECENT, 0.1 s: seconds_since_last_valid_sample() at NOW + RECENT, 100.1 s
The last clock check
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2✕ NO_CLOCK: a sample stamped NOW − SHORTLY_BEFORE, 99.99 s, at ROS_TIME_ZERO, 0 s
- 3returns None: seconds_since_last_valid_sample() at NOW − ONE_MILLISECOND, 99.999 s
The two tests
A sample at the ROS time 0 returns NO_CLOCK before the clock check and leaves the validator's attributes as they are. A test in Before the checks: no clock checks this for last_valid_stamp. The next two tests check the other two attributes, last_valid_arrival_time and last_checked_ros_time. Both give the validator the same two samples, a valid one at NOW and one at the ROS time 0. They differ in the ROS time at which they ask for the silence:
The excerpt shows the two tests, from test_imu_validator.py, lines 244–260:
| test_imu_validator.py: the silence after a sample at 0 s | |
|---|---|
244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 | |
- The arrival time: a valid sample arrives at
NOW, and a sample stamped 99.99 s,SHORTLY_BEFOREbeforeNOW, returnsNO_CLOCKatROS_TIME_ZERO. At 100.1 s,RECENTafterNOW, the method returns 0.1 s, counted from the arrival atNOW. A validator that clearedlast_valid_arrival_timeat 0 s would returnNone, and one that stored the stamp 99.99 s there would return 0.11 s. - The last clock check: the same two calls, then the method is asked at 99.999 s,
ONE_MILLISECONDbeforeNOW.last_checked_ros_timeis still 100 s, so the clock went back, and the method returnsNone. A validator that stored 0 s or the stamp 99.99 s as the last clock check would find no clock that went back and return −0.001 s.
After the clock goes back
The next three tests are about a clock that goes back while no valid sample arrives. In each test, a valid sample arrives at NOW, and a later call has an earlier ROS time:
Without samples
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2returns None: seconds_since_last_valid_sample() at RESTART, 50 s
- 3returns None: seconds_since_last_valid_sample() at RESTART + SILENCE, 53.5 s
Back by less than the silence
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2returns SILENCE, 3.5 s: seconds_since_last_valid_sample() at NOW + SILENCE, 103.5 s
- 3returns None: seconds_since_last_valid_sample() at NOW + RECENT, 100.1 s
- 4returns None: seconds_since_last_valid_sample() at NOW + ONE_SECOND, 101 s
Shown by a rejected sample
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2returns SILENCE, 3.5 s: seconds_since_last_valid_sample() at NOW + SILENCE, 103.5 s
- 3✕ TOO_OLD: a sample stamped NOW − ONE_SECOND, 99 s, at NOW + RECENT, 100.1 s
- 4returns None: seconds_since_last_valid_sample() at NOW + SILENCE + ONE_SECOND, 104.5 s
The three tests
The excerpt shows the three tests, from test_imu_validator.py, lines 262–288:
| test_imu_validator.py: the silence after the clock goes back | |
|---|---|
262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 | |
- Without samples: a valid sample arrives at the ROS time
NOW, 100 s. Then the method is asked at the ROS timeRESTART, 50 s, andSILENCElater, at 53.5 s, and returnsNoneboth times. The method itself notices that the clock went back, because the method checks the clock withclear_last_valid_sample_times_if_clock_went_back()in each call. - Back by less than the silence: a valid sample arrives at the ROS time
NOW, 100 s. The method is asked at the ROS time 103.5 s,SILENCEafterNOW, and returns a silence of 3.5 s. Then the method is asked at 100.1 s,NOWplusRECENT. The clock went back by 3.4 s, less than the silence, so the new ROS time is still later than the arrival of the valid sample, and \(t - t_\text{valid}\) would be 0.1 s, a silence thathealth()reports as OK. The method returnsNoneall the same, and at 101 s,ONE_SECONDafterNOW, as well. A sample from before the clock went back says nothing about the IMU in the new time. - Shown by a rejected sample: a valid sample arrives at the ROS time
NOW, 100 s, and the method returns a silence of 3.5 s at 103.5 s. Then the clock goes back. The first call in the new time is a sample that arrives at 100.1 s,NOWplusRECENT, stamped 99 s,ONE_SECONDbeforeNOW, which the validator rejects as too old. The validator checks the clock also for a sample that a check rejects, so in this call the validator sets its attributeslast_valid_stampandlast_valid_arrival_timetoNone. The method returnsNoneat 104.5 s,SILENCEplusONE_SECONDafterNOW. This last call is later than the call at 103.5 s, so its own clock check finds no clock that went back. Only the rejected sample's call can have cleared the two attributes. A sample at the ROS time 0 would not show that the clock went back: the validator rejects such a sample before the clock check.
The first valid sample after the clock went back
The last test of the class follows the silence through a whole restart, from a valid sample before the clock goes back to the first valid sample after it:
The silence starts again
- 1✓ accepted: a sample stamped NOW, 100 s, at NOW, 100 s
- 2✕ FROM_THE_FUTURE: a sample stamped NOW + MAX_FUTURE, 100.02 s, at RESTART, 50 s
- 3✕ WRONG_FRAME: a sample in the frame base_link stamped RESTART, 50 s, at RESTART, 50 s
- 4returns None: seconds_since_last_valid_sample() at RESTART + ONE_SECOND, 51 s
- 5✓ accepted: a sample stamped RESTART + SILENCE, 53.5 s, at RESTART + SILENCE, 53.5 s
- 6returns RECENT, 0.1 s: seconds_since_last_valid_sample() at RESTART + SILENCE + RECENT, 53.6 s
The test
The excerpt shows the test, from test_imu_validator.py, lines 290–303:
| test_imu_validator.py: the silence starts again | |
|---|---|
290 291 292 293 294 295 296 297 298 299 300 301 302 303 | |
A valid sample arrives at the ROS time NOW, 100 s. Then the ROS time is RESTART, 50 s. At this ROS time, the validator rejects a sample from the future and a sample with the wrong frame. At 51 s, ONE_SECOND after RESTART, the method still returns None. At 53.5 s, SILENCE after RESTART, a valid sample arrives. At 53.6 s, RECENT, 0.1 s, after that sample, the method returns 0.1 s.
TestHealth: the status
health() turns the silence and the number of samples dropped since the report before into the level of the diagnostics status. A silence error comes before the warning about new drops. The table shows the level that each of the five tests of TestHealth expects:
| Silence | New drops | Level that the test expects |
|---|---|---|
None: no valid sample yet |
0 | ERROR |
SILENCE_LIMIT, 1 s |
0 | OK |
SILENCE_LIMIT plus ONE_MILLISECOND, 1.001 s |
0 | ERROR |
RECENT, 0.1 s |
NEW_DROP_COUNT, 3 |
WARN, and the message contains the number 3 |
RECENT, 0.1 s |
0 | OK |
None, and 1.001 s |
3 | ERROR for both: silence comes before the warning |
One test checks the limit of the silence. The excerpt shows it, from test_imu_validator.py, lines 312–316:
| test_imu_validator.py: the silence limit and 1 ms beyond | |
|---|---|
312 313 314 315 316 | |
With no new drops, a silence of exactly SILENCE_LIMIT, 1 s, is still OK, and 1 ms more is ERROR. The test reads the limit from imu_validator, so it doesn't repeat its value. The validator's diagnostics says what the levels mean on the robot.
The function health()
health() returns a Health, a named tuple of the level and the message. The node calls the function once for each diagnostics report, with the numbers of one snapshot. The excerpt shows the function, from imu_validator.py, lines 143–160:
| imu_validator.py: health() | |
|---|---|
143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 | |
The function looks at three conditions in their order and returns at the first one that holds:
- No valid sample: the silence is
None, before the first valid sample or since the clock went back. The function returnsERROR. - A silence that is too long: the silence is longer than
SILENCE_LIMIT, 1 s. The function returnsERROR, with the silence in the message. - New drops:
new_drop_countis more than 0. The function returnsWARN, with the number in the message.
When none of the three holds, the function returns OK. The function reads no clock and keeps no state, so each call is independent of the others.
All five tests
The excerpt shows the class's five tests, from test_imu_validator.py, lines 309–331:
| test_imu_validator.py: the level of the status | |
|---|---|
309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 | |
TestSampleCounts: the numbers of a report
SampleCounts counts the forwarded and the dropped samples. For each diagnostics report, take_snapshot() returns the numbers of one report, among them new_drop_count, the samples dropped since the report before. The four tests:
test_snapshot_has_the_forwarded_count_and_the_drops_by_reason: the counts by reason, and the silence that the test passed.test_snapshot_counts_a_dropped_sample_as_new_only_once: a dropped sample is new in the next snapshot and not in the one after.test_snapshot_counts_a_dropped_sample_as_new_once_also_in_an_error_report: the same whenhealth()reports an error.test_snapshot_keeps_its_numbers_when_more_samples_arrive: a report's numbers all come from one moment.
SampleCounts and take_snapshot()
SampleCounts counts the forwarded samples and the dropped samples by their reason, without ROS communication. The node's callback on_sample() calls count_forwarded() for each forwarded sample and count_dropped() for each dropped one. For each diagnostics report, the node calls the method take_snapshot(), which returns the numbers of one report as a DiagnosticSnapshot. This named tuple has four fields: forwarded_count, drops_by_reason, new_drop_count and seconds_since_last_valid_sample. The excerpt shows the class, from imu_validator.py, lines 163–190:
| imu_validator.py: SampleCounts | |
|---|---|
163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 | |
take_snapshot() counts the new drops: the method adds up all dropped samples, subtracts drop_count_at_last_snapshot, the number that the snapshot before counted, and stores the new total for the next snapshot. The method copies drops_by_reason into a new dictionary, so a snapshot keeps its numbers when more samples arrive.
The four tests
The excerpt shows the first two tests, from test_imu_validator.py, lines 340–358:
| test_imu_validator.py: the counts of a snapshot | |
|---|---|
340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 | |
- The counts: after two forwarded samples, two samples dropped as too old and one dropped for its frame, the snapshot has
forwarded_count2 anddrops_by_reasonwith 2 and 1. The snapshot also carries the silence that the test passed totake_snapshot(). - New drops:
new_drop_countis the number of samples dropped since the snapshot before. One dropped sample makes this number 1 in the next snapshot and 0 in the one after. Two more dropped samples, for two different reasons, make the number 2.health()uses this number for the report's level: a new drop can raise the next report toWARNwhen the IMU is otherwise healthy. The report after it isOKagain if no further sample was dropped and the IMU is still healthy. A silence that is too long, or no valid sample yet, takes priority: the report is thenERROR, as the last row of the table inTestHealthshows.
The other two tests cover two cases in which this count could go wrong. The excerpt shows them, from test_imu_validator.py, lines 360–376:
| test_imu_validator.py: a snapshot in an error report, and a snapshot that stays | |
|---|---|
360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 | |
- In an error report: a sample is dropped before any sample was valid. The first snapshot gets the silence
None, sohealth()reports an error, not the warning. The dropped sample counts as new in this snapshot all the same, and not again in the next one. - A snapshot stays as it is: after
take_snapshot(), the test counts one more forwarded sample and one more dropped sample. The snapshot from before still has its numbers. A report is built from one snapshot, so its numbers all come from one moment.
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.