Skip to content

Test: The ekf Switch of the Launch Files

The ekf argument has to do several things at once in several launch files, as Which launch file starts what shows. With ekf:=true, a launch file that starts the robot or the simulation starts the EKF and the IMU sample validator and turns off the drive controller's transform odom → base_footprint. A launch file that starts a planner makes the planner read the EKF's odometry by default. diffbot_navigation's move_base.launch also takes the argument odom_topic, which names another topic. Without ekf:=true, none of this may happen. The test proves that the launch files are written this way, for both positions of the switch and for its default. The test reads the launch files and starts nothing. So the test doesn't prove that the nodes run or that they work together. The stepped-clock test and the test in Gazebo start them.

Two launch files, read without starting them

roslaunch has a loader that reads a launch file and everything that the file includes, and returns what roslaunch would start: the nodes, the parameters and the remappings. The test calls this loader and looks at the result. No ROS master and no node run. Two examples show what the loader returns, in a run at the commit 27d72a4 of the diffbot repository:

  1. A launch file that starts the simulation: for roslaunch diffbot_gazebo diffbot.launch ekf:=true, the loader returns nine nodes. Two of them are the EKF, /diffbot/ekf_localization, and the IMU sample validator, /diffbot/imu_validator. The parameter /diffbot/mobile_base_controller/enable_odom_tf is False, so the drive controller doesn't publish the transform odom → base_footprint. Without the ekf argument, the loader returns seven nodes, without these two, and enable_odom_tf is True.
  2. A launch file that starts only a planner: for roslaunch diffbot_navigation move_base.launch ekf:=true, the loader returns one node, the planner /move_base. The planner's name odom is remapped to /diffbot/odometry/filtered, the EKF's odometry. Without the ekf argument, odom is remapped to /diffbot/mobile_base_controller/odom, the drive controller's wheel odometry. This launch file starts no EKF and sets no enable_odom_tf, so the test checks only the planner's topic here.

The figure shows this flow from top to bottom, for the first example. The test's function load() gives the launch file and its arguments to roslaunch's loader, load_config_default(). load() returns the loader's result as a LaunchConfiguration with three fields, and the test's assertions look at these fields. The box at the bottom left names one of these assertions for the first example. The dashed box is what roslaunch would do after loading, which the test leaves out.

How test_ekf_switch.py works: load() gives a launch file and its arguments to roslaunch's loader, load_config_default(). The loader returns what roslaunch would start, which the test keeps as a LaunchConfiguration with the nodes, the parameters and a description. The test's assertions look at this result. The ROS master and the nodes are not started launch file with arguments diffbot_gazebo diffbot.launch ekf:=true load() roslaunch's loader load_config_default() reads the file and its includes what roslaunch would start LaunchConfiguration nodes parameters description the test's assertions for example enable_odom_tf: False ROS master, nodes not started
  • what roslaunch would do next and the test leaves out
How test_ekf_switch.py works, from top to bottom, for one of its launch files.

Run it

Run the test alone from a shell in the built workspace of the dev container:

1
2
3
4
cd ~/catkin_ws
source devel/setup.bash
cd src/diffbot/diffbot_gazebo/test
python3 -m unittest -v test_ekf_switch

The test needs no ROS master and no Gazebo. A pass prints one line for each of the five tests, each ending in ok, and OK at the end. catkin runs the test with the repository's other tests.

The registration, the output of a run and the test dependencies

catkin runs the test with the repository's other tests. The excerpt shows where diffbot_gazebo registers the test, from CMakeLists.txt, lines 225–226:

diffbot_gazebo/CMakeLists.txt: the test's registration
225
226
# The ekf switch in the launch files, both ways, without starting them
catkin_add_nosetests(test/test_ekf_switch.py)

The test needs no ROS master and no Gazebo. In a run at the commit 27d72a4 in the dev container's image, the test printed one line for each of its five tests, and OK at the end because every test passed:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
test_bringup_starts_the_ekf_and_the_validator_only_with_ekf_true (test_ekf_switch.TestEkfSwitch) ... ok
test_navigation_simulation_starts_the_ekf_for_its_planner_with_ekf_true (test_ekf_switch.TestEkfSwitch) ... ok
test_planner_reads_an_explicit_odom_topic_also_with_ekf_true (test_ekf_switch.TestEkfSwitch) ... ok
test_planners_read_the_filtered_odometry_only_with_ekf_true (test_ekf_switch.TestEkfSwitch) ... ok
test_simulation_starts_the_ekf_and_the_validator_only_with_ekf_true (test_ekf_switch.TestEkfSwitch) ... ok

----------------------------------------------------------------------
Ran 5 tests in 6.574s

OK

The test loads the launch files of diffbot_bringup, diffbot_navigation and diffbot_mbf, so diffbot_gazebo's package.xml lists these three packages as test dependencies.

What a pass proves and what a failure means

A pass proves these results of roslaunch's loader:

  • The robot and the simulation: with ekf:=true, diffbot_bringup's two launch files and the two simulation files diffbot.launch start the EKF and the IMU sample validator and turn off the drive controller's transform. With ekf:=false and by default, they start neither node and keep the transform on.
  • The planners: the four planner launch files of the table give the planner the EKF's odometry with ekf:=true and the wheel odometry by default. move_base.launch keeps an explicit odom_topic also with ekf:=true.
  • The simulation with navigation: with ekf:=true, diffbot_navigation's diffbot.launch starts the EKF and the validator, turns off the controller's transform and gives its planner the EKF's odometry.

A failure's message includes the launch file with its arguments and names what is missing or wrong, such as a node that doesn't start or the planner's odom remapped to another topic.

What the test doesn't show

  • Running nodes: the loader reads the launch files. Nothing starts, so the test can't see whether a node runs, publishes or finds its transforms.
  • robot_state_publisher in every file: the test asserts robot_state_publisher for the two bringup files and for the two simulation files named diffbot.launch, of diffbot_gazebo and of diffbot_control. The test doesn't assert the node for minimal.launch, which doesn't start robot_state_publisher.
  • ekf:=false for the planners: for the planners' launch files, the test loads the default and ekf:=true.
  • Other ways to start the robot: the robot container starts bringup.launch without ekf. The test doesn't read the container's settings.

The five tests

test_ekf_switch.py has five tests. Each test loads one kind of launch file:

Test Launch files What it checks
test_bringup_starts_the_ekf_and_the_validator_only_with_ekf_true diffbot_bringup: bringup.launch, bringup_with_laser.launch Both positions of the switch
test_simulation_starts_the_ekf_and_the_validator_only_with_ekf_true diffbot_gazebo: diffbot.launch. diffbot_control: diffbot.launch Both positions of the switch
test_planners_read_the_filtered_odometry_only_with_ekf_true diffbot_navigation: move_base.launch, diffbot_navigation.launch, diffbot_hw.launch. diffbot_mbf: amcl_demo.launch The planner's odometry topic
test_planner_reads_an_explicit_odom_topic_also_with_ekf_true diffbot_navigation: move_base.launch The planner's odometry topic
test_navigation_simulation_starts_the_ekf_for_its_planner_with_ekf_true diffbot_navigation: diffbot.launch The simulation with navigation

The two simulation files named diffbot.launch are different files. diffbot_gazebo/launch/diffbot.launch starts the simulation without RViz. diffbot_control/launch/diffbot.launch starts the same simulation and RViz.

The next table lists what the test asserts. Each group of rows is one kind of launch file. A row is one way in which the test loads the files of its group: on the left the launch arguments, on the right what the test asserts about the nodes and the parameters that roslaunch would start. The table uses three short forms:

  • The nodes: the table leaves out the namespace /diffbot of the nodes ekf_localization, imu_validator and robot_state_publisher.
  • A planner reads a topic: the planner's node is among the nodes, and the node's name odom is remapped to this topic.
  • The controller's odometry and the EKF's odometry: the topics /diffbot/mobile_base_controller/odom and /diffbot/odometry/filtered, the test's WHEEL_ODOMETRY_TOPIC and FILTERED_TOPIC.
ArgumentsWhat the test asserts
The launch files that start the robot or the simulation: bringup.launch and bringup_with_laser.launch of diffbot_bringup, diffbot.launch of diffbot_gazebo, diffbot.launch of diffbot_control
ekf:=false, and no ekf argumentNeither ekf_localization nor imu_validator is among the nodes.
enable_odom_tf is True.
ekf:=trueekf_localization and imu_validator are among the nodes.
enable_odom_tf is False.
The five parameters in EKF_PARAMETERS have their values.
robot_state_publisher is among the nodes.
The launch files that start a planner: move_base.launch, diffbot_navigation.launch and diffbot_hw.launch of diffbot_navigation, which start the planner /move_base, and amcl_demo.launch of diffbot_mbf, which starts the planner /move_base_flex
no ekf argumentThe planner reads the controller's odometry.
ekf:=trueThe planner reads the EKF's odometry.
ekf:=true and an explicit odom_topicOnly move_base.launch, with odom_topic:=/other/odom: the planner reads the topic /other/odom.
The launch file that starts the simulation and a planner: diffbot.launch of diffbot_navigation, loaded with open_rviz:=false
ekf:=trueekf_localization and imu_validator are among the nodes.
enable_odom_tf is False.
The planner /move_base reads the EKF's odometry.

Reading a launch file without starting it

The excerpt shows the test's own result type and the function that calls the loader, from test_ekf_switch.py, lines 55–74:

test_ekf_switch.py: LaunchConfiguration and load()
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
class LaunchConfiguration(NamedTuple):
    """What roslaunch would start: the nodes by their full name, and the parameters."""

    nodes: Dict[str, Node]
    parameters: Dict[str, Any]
    description: str  # the launch file and its arguments, for failure messages


def load(package: str, launch_file: str, **args: str) -> LaunchConfiguration:
    """Return what roslaunch would start for the package's launch file with the arguments."""
    path = os.path.join(PACKAGES.get_path(package), 'launch', launch_file)
    argv = ['{}:={}'.format(name, value) for name, value in args.items()]
    config = load_config_default([(path, argv)], None, verbose=False)
    nodes = {}
    for node in config.nodes:
        name = '/' + '/'.join(part for part in (node.namespace.strip('/'), node.name) if part)
        nodes[name] = node
    parameters = {name: parameter.value for name, parameter in config.params.items()}
    description = ' '.join([package, launch_file] + argv)
    return LaunchConfiguration(nodes, parameters, description)
The result's fields, and the constants
  • load(): takes a package, the name of a launch file in the package's launch folder and the launch arguments. load('diffbot_gazebo', 'diffbot.launch', ekf='true') stands for the command roslaunch diffbot_gazebo diffbot.launch ekf:=true.
  • load_config_default(): is roslaunch's loader. The loader resolves the includes, the arguments and the conditions such as if="$(arg ekf)", as roslaunch does before it starts anything.
  • nodes: a dictionary from each node's full name, for example /diffbot/ekf_localization, to roslaunch's description of the node. The loader keeps a node's namespace and its name apart, so load() joins them.
  • parameters: a dictionary from each parameter's full name to its value.
  • description: the launch file with its arguments, as text. Every failure message includes this text.

The constants of test_ekf_switch.py:

The excerpt shows the names that the test looks for, from test_ekf_switch.py, lines 31–50. The name of a constant ends in _NODE for a node, in _TOPIC for a topic and in _PARAMETER for a parameter. SWITCHED_OFF holds the two ways to run without the EKF, ekf:=false and no ekf argument. EKF_PARAMETERS holds five parameters of the EKF and of the validator with the values that ekf.yaml and ekf.launch give them.

test_ekf_switch.py: the constants
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
EKF_NODE = '/diffbot/ekf_localization'
VALIDATOR_NODE = '/diffbot/imu_validator'
STATE_PUBLISHER_NODE = '/diffbot/robot_state_publisher'
ODOM_TF_PARAMETER = '/diffbot/mobile_base_controller/enable_odom_tf'
WHEEL_ODOMETRY_TOPIC = '/diffbot/mobile_base_controller/odom'
FILTERED_TOPIC = '/diffbot/odometry/filtered'
MOVE_BASE_NODE = '/move_base'
MOVE_BASE_FLEX_NODE = '/move_base_flex'
# The two ways to run without the EKF: ekf:=false, and no ekf argument, the default
SWITCHED_OFF: List[Dict[str, str]] = [{'ekf': 'false'}, {}]
# ekf.yaml's frames and inputs in the EKF's private namespace, and the validator's frame: the
# filter works in odom, for base_footprint, on the controller's odometry and the validator's
# output
EKF_PARAMETERS = {
    EKF_NODE + '/base_link_frame': 'base_footprint',
    EKF_NODE + '/world_frame': 'odom',
    EKF_NODE + '/odom0': 'mobile_base_controller/odom',
    EKF_NODE + '/imu0': 'imu/data_validated',
    VALIDATOR_NODE + '/frame_id': 'imu_link',
}

Both positions of the switch

The first two tests call assert_ekf_switch() for the four launch files that start the robot or the simulation. assert_ekf_on() checks the switch with ekf:=true, and assert_ekf_off() with ekf:=false and without the argument.

The two tests, the checks on and off, and the assertion methods

The excerpt shows the two tests, from test_ekf_switch.py, lines 166–172:

test_ekf_switch.py: the bringup files and the two simulation files
166
167
168
169
170
171
172
def test_bringup_starts_the_ekf_and_the_validator_only_with_ekf_true(self) -> None:
    self.assert_ekf_switch('diffbot_bringup', 'bringup.launch')
    self.assert_ekf_switch('diffbot_bringup', 'bringup_with_laser.launch')

def test_simulation_starts_the_ekf_and_the_validator_only_with_ekf_true(self) -> None:
    self.assert_ekf_switch('diffbot_gazebo', 'diffbot.launch')
    self.assert_ekf_switch('diffbot_control', 'diffbot.launch')

assert_ekf_switch() checks the switch in both positions. The excerpt shows assert_ekf_on(), assert_ekf_off() and assert_ekf_switch(), from test_ekf_switch.py, lines 131–149:

test_ekf_switch.py: the switch on, off, and both
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
def assert_ekf_on(self, package: str, launch_file: str) -> None:
    """Assert everything that ekf:=true has to do in the package's launch file."""
    configuration = load(package, launch_file, ekf='true')
    self.assert_ekf_nodes_start(configuration)
    self.assert_controller_odom_tf(configuration, enabled=False)
    self.assert_ekf_reads_the_controller_and_the_validator(configuration)
    self.assert_state_publisher_starts(configuration)

def assert_ekf_off(self, package: str, launch_file: str) -> None:
    """Assert that the launch file runs without the EKF, with ekf:=false and by default."""
    for switch in SWITCHED_OFF:
        configuration = load(package, launch_file, **switch)
        self.assert_no_ekf_nodes_start(configuration)
        self.assert_controller_odom_tf(configuration, enabled=True)

def assert_ekf_switch(self, package: str, launch_file: str) -> None:
    """Assert both positions of the switch in the package's launch file."""
    self.assert_ekf_off(package, launch_file)
    self.assert_ekf_on(package, launch_file)

Off, assert_ekf_off(), once with ekf:=false and once without the argument:

  1. assert_no_ekf_nodes_start(): neither EKF_NODE, which is /diffbot/ekf_localization, nor VALIDATOR_NODE, which is /diffbot/imu_validator, is among the nodes.
  2. assert_controller_odom_tf(configuration, enabled=True): the parameter ODOM_TF_PARAMETER, which is /diffbot/mobile_base_controller/enable_odom_tf, is set, and its value is True. So the drive controller publishes the transform.

On, assert_ekf_on(), with ekf:=true:

  1. assert_ekf_nodes_start(): both nodes are among the nodes, EKF_NODE from the package robot_localization with the executable ekf_localization_node, and VALIDATOR_NODE from diffbot_control with imu_validator.py.
  2. assert_controller_odom_tf(configuration, enabled=False): enable_odom_tf is set, and its value is False. So only the EKF publishes the transform.
  3. assert_ekf_reads_the_controller_and_the_validator(): the five parameters in EKF_PARAMETERS have their values. The EKF works in the frame odom for the frame base_footprint, reads the topics mobile_base_controller/odom and imu/data_validated, and the validator expects the frame imu_link.
  4. assert_state_publisher_starts(): STATE_PUBLISHER_NODE, which is /diffbot/robot_state_publisher, is among the nodes. The EKF needs the transform between imu_link and base_footprint, which TF has from robot_state_publisher.

The assertion methods on the nodes and the parameters:

The excerpt shows the methods that assert_ekf_on() and assert_ekf_off() call, from test_ekf_switch.py, lines 85–129. assert_controller_odom_tf() uses assertIs(): a parameter that the launch file doesn't set gives None, which is neither True nor False, so the check fails for a missing parameter in both positions.

test_ekf_switch.py: the assertions on the nodes and the parameters
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
def assert_node(self, configuration: LaunchConfiguration, name: str, package: str,
                node_type: str) -> None:
    """Assert that the launch file starts the node name from package's node_type."""
    self.assertIn(name, sorted(configuration.nodes),
                  "{} doesn't start {}".format(configuration.description, name))
    node = configuration.nodes[name]
    self.assertEqual((package, node_type), (node.package, node.type),
                     '{}: the executable of {}'.format(configuration.description, name))

def assert_ekf_nodes_start(self, configuration: LaunchConfiguration) -> None:
    """Assert that the launch file starts the EKF and the IMU sample validator."""
    self.assert_node(configuration, EKF_NODE, 'robot_localization', 'ekf_localization_node')
    self.assert_node(configuration, VALIDATOR_NODE, 'diffbot_control', 'imu_validator.py')

def assert_no_ekf_nodes_start(self, configuration: LaunchConfiguration) -> None:
    """Assert that the launch file starts neither the EKF nor the IMU sample validator."""
    for name in (EKF_NODE, VALIDATOR_NODE):
        self.assertNotIn(name, sorted(configuration.nodes),
                         '{} starts {}'.format(configuration.description, name))

def assert_controller_odom_tf(self, configuration: LaunchConfiguration,
                              enabled: bool) -> None:
    """Assert that the launch file sets the controller's enable_odom_tf to enabled.

    Set, not left out: roslaunch writes each parameter at each start, so a false value from
    an earlier run with the EKF on the same ROS master doesn't remain.
    """
    self.assertIs(enabled, configuration.parameters.get(ODOM_TF_PARAMETER),
                  "{}: the controller's enable_odom_tf".format(configuration.description))

def assert_ekf_reads_the_controller_and_the_validator(
        self, configuration: LaunchConfiguration) -> None:
    """Assert the EKF's frames and inputs and the validator's frame (EKF_PARAMETERS)."""
    for name, value in EKF_PARAMETERS.items():
        self.assertEqual(value, configuration.parameters.get(name),
                         '{}: {}'.format(configuration.description, name))

def assert_state_publisher_starts(self, configuration: LaunchConfiguration) -> None:
    """Assert that the launch file starts robot_state_publisher.

    The EKF looks up imu_link → base_footprint, which TF has from robot_state_publisher.
    """
    self.assertIn(STATE_PUBLISHER_NODE, sorted(configuration.nodes),
                  '{} starts the EKF without robot_state_publisher'.format(
                      configuration.description))

The launch file has to write enable_odom_tf in both positions, because a parameter stays on the ROS parameter server as long as the ROS master runs. A false from a launch with the EKF would otherwise remain for a later launch without it.

The planner's odometry topic

A planner reads the robot's velocity from an odometry topic. With the EKF on, the planner has to read the EKF's estimate, so that its velocity and the transform come from the same estimate. The test checks the remapping of the planner's name odom with ekf:=true and without it, and that an explicit odom_topic wins.

assert_planner_follows_the_switch() loads a planner's launch file without the ekf argument and with ekf:=true. The excerpt shows it, from test_ekf_switch.py, lines 159–164:

test_ekf_switch.py: assert_planner_follows_the_switch()
159
160
161
162
163
164
def assert_planner_follows_the_switch(self, package: str, launch_file: str,
                                      node_name: str) -> None:
    """Assert that the planner reads the EKF's odometry with ekf:=true only."""
    self.assert_planner_odometry(load(package, launch_file), node_name, WHEEL_ODOMETRY_TOPIC)
    self.assert_planner_odometry(load(package, launch_file, ekf='true'), node_name,
                                 FILTERED_TOPIC)

Without the argument, the planner has to read WHEEL_ODOMETRY_TOPIC, the topic /diffbot/mobile_base_controller/odom. With ekf:=true, it has to read FILTERED_TOPIC, the topic /diffbot/odometry/filtered.

The check of one planner, the helper remapping() and the two tests

In a launch file, the planner's node gets the topic as a remapping of the name odom. The excerpt shows what the test asserts about this remapping, from test_ekf_switch.py, lines 151–157:

test_ekf_switch.py: assert_planner_odometry()
151
152
153
154
155
156
157
def assert_planner_odometry(self, configuration: LaunchConfiguration, node_name: str,
                            topic: str) -> None:
    """Assert that the planner node_name reads its odometry from topic."""
    self.assertIn(node_name, sorted(configuration.nodes),
                  "{} doesn't start {}".format(configuration.description, node_name))
    self.assertEqual(topic, remapping(configuration.nodes[node_name], 'odom'),
                     "{}: {}'s odom".format(configuration.description, node_name))

assert_planner_odometry() checks that the planner's node is among the nodes and that its name odom is remapped to topic.

assert_planner_odometry() reads the remapping with the helper remapping(). The excerpt shows the helper, from test_ekf_switch.py, lines 77–79:

test_ekf_switch.py: remapping()
77
78
79
def remapping(node: Node, name: str) -> Optional[str]:
    """Return what the node's topic name is remapped to, or None."""
    return dict(node.remap_args).get(name)

roslaunch's description of a node holds the node's remappings as pairs of the name and what the name is remapped to. remapping() returns the second part for name, or None when the node has no such remapping.

The excerpt shows the two tests that use these methods, from test_ekf_switch.py, lines 174–187:

test_ekf_switch.py: the planners
174
175
176
177
178
179
180
181
182
183
184
185
186
187
def test_planners_read_the_filtered_odometry_only_with_ekf_true(self) -> None:
    self.assert_planner_follows_the_switch('diffbot_navigation', 'move_base.launch',
                                           MOVE_BASE_NODE)
    self.assert_planner_follows_the_switch('diffbot_navigation', 'diffbot_navigation.launch',
                                           MOVE_BASE_NODE)
    self.assert_planner_follows_the_switch('diffbot_navigation', 'diffbot_hw.launch',
                                           MOVE_BASE_NODE)
    self.assert_planner_follows_the_switch('diffbot_mbf', 'amcl_demo.launch',
                                           MOVE_BASE_FLEX_NODE)

def test_planner_reads_an_explicit_odom_topic_also_with_ekf_true(self) -> None:
    configuration = load('diffbot_navigation', 'move_base.launch', ekf='true',
                         odom_topic='/other/odom')
    self.assert_planner_odometry(configuration, MOVE_BASE_NODE, '/other/odom')

These launch files start no EKF, so the test checks nothing else in them.

The simulation with navigation

diffbot_navigation's diffbot.launch starts the simulation and a planner together and passes ekf to both.

The test

The excerpt shows the test, from test_ekf_switch.py, lines 189–194:

test_ekf_switch.py: the simulation with navigation
189
190
191
192
193
194
def test_navigation_simulation_starts_the_ekf_for_its_planner_with_ekf_true(self) -> None:
    configuration = load('diffbot_navigation', 'diffbot.launch', ekf='true',
                         open_rviz='false')
    self.assert_ekf_nodes_start(configuration)
    self.assert_controller_odom_tf(configuration, enabled=False)
    self.assert_planner_odometry(configuration, MOVE_BASE_NODE, FILTERED_TOPIC)

With ekf:=true, the EKF and the validator start, enable_odom_tf is False, and /move_base reads FILTERED_TOPIC, the topic /diffbot/odometry/filtered. open_rviz:=false leaves RViz out of what the loader reads.

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.