An Integrated Modeling and TestingArchitecture for ROS Nodes
Jacob Hartzer
ROSCon 2023
1

Unmanned Systems Lab• On/Off Road Autonomy• LIDAR and RADAR Odometry and Perception• Multi-Sensor Fusion and Calibration

Integrated Modeling and Testing of ROS Nodes

2

Our Work - Calibration••••

Utilize a wide variety of sensors and environments
RTK GPS in highway applications
Ultra-Wideband for cooperative localization and detection
Multi-IMU, Multi-Camera fusion

Integrated Modeling and Testing of ROS Nodes

3

The Challenge of Calibration•••••Sensors do not work well when uncalibrated
Ideally would like to calibrate sensors online
Re-calibrating on the fly is more robust
Proving stability and consistency is hardSensor flexibility makes it even harder

Must test many, many
datasets and environments

Integrated Modeling and Testing of ROS Nodes

4

EKF-CAL Package
•
•
•
•
•

EKF-CAL is flexible MSCKF-based sensor calibration package
Inputs are YAML based and compatible with ROS 2 parameter declarations
Inherently multi-sensor (IMU, Camera, and GPS soon)
Developed with integrated testing and Monte Carlo simulation in mind
Open Source!

Integrated Modeling and Testing of ROS Nodes

5

Typical Development
Need performance / Real-Time

Develop Algorithm inscripted language(MatLab, Julia, etc.)

Deploy code incompiled language(C++, Rust, etc.)

Find a bug / invalid assumptions
Integrated Modeling and Testing of ROS Nodes

6

Typical Development
Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

Preprocessors

Sensor Interfaces

Low-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment7

Typical Development
Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

Preprocessors

Sensor Interfaces

Low-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment8

Typical Development
Simulated Algorithm

Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
9

Typical Development
Simulated Algorithm

Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

PreprocessorsPreprocessors

High-Fidelity
Measurements

Sensor Interfaces

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
10

Typical Development
Simulated Algorithm

Main Algorithm

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

High-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment11

Typical Development
Simulated Algorithm

Main Algorithm

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

High-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

TrueMeasurements
True Deployment
12

Typical Development
Functionally Identical
Simulated Algorithm

Main Algorithm

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

High-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
13

Typical Development
Functionally Identical

Issues:Simulated Algorithm
Main Algorithm
 Very iterative development
 Time-intensive
and tedious work Preprocessors
Preprocessors
 Consistent work to maintain simulation
High-Fidelity
Measurements

High-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

Sensor Interfaces
True
Measurements
True Deployment
14

Typical Development
Simulated Algorithm

Main Algorithm

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

High-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
15

Typical Development
Simulated Algorithm

Main Algorithm

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

High-Fidelity Simulation
Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
16

Typical Development
Simulated Algorithm

Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
17

Typical Development
Three Code Bases
Simulated Algorithm

Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

Preprocessors

Preprocessors

High-Fidelity
Measurements

Sensor Interfaces

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment18

Typical Development
Three Code Bases

Result:Simulated Algorithm
Simulated Algorithm
Main Algorithm
 Complex simulations lead to fragmented code
Low-Fidelity
 Fragmented
code isPreprocessors
expensive to maintain
Preprocessors
Measurements
 Multiple simulations leads to:High-Fidelity
 Uncaught bugs
Sensor Interfaces
Measurements
 Untested deployment code

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
19

Typical Development
Simulated Algorithm

Simulated Algorithm

Main Algorithm

Low-Fidelity
Measurements

Preprocessors

Preprocessors

High-FidelityMeasurements

Sensor Interfaces

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

True
Measurements
True Deployment
20

Proposed Solution
Algorithm
Low-Fidelity
Measurements

Preprocessors
High-Fidelity
Measurements

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

Sensor Interfaces
True
Measurements
True Deployment
21

Proposed Solution

Abstraction Layers!
Algorithm

Measurement Processor Abstraction

Low-Fidelity
Measurements

Preprocessors
Interface Abstraction

High-Fidelity
Measurements

Sensor Interfaces

Hardware Abstraction

Low-Fidelity Simulation

High-Fidelity Simulation

Integrated Modeling and Testing of ROS Nodes

TrueMeasurements
True Deployment
22

Cost of Integrated Coding

Pays off in long-run

Higher initial cost

Integrated Modeling and Testing of ROS Nodes

23

Abstraction - Example• Sensors call updates to filter / algorithm

Utilize real or simulated sensor messages• Feature tracker utilizes camera measurements
True deployment utilizes true camera measurements
High-Fidelity simulation provides ray-traced images
Low-Fidelity simulation provides “pre-tracked features”

Integrated Modeling and Testing of ROS Nodes

24

Abstraction - Example
TruthEngine.cpp
GetState(double time);

SimCamera.cpp
GenerateImages();

SimFeatureTracker.cpp
GenerateTracks();

Camera.cpp
ReadImages();

FeatureTracker.cpp
TrackFeatures();

Integrated Modeling and Testing of ROS Nodes

ekf.cpp
PredictFilter();
UpdateMSCKF();
25

Abstraction - Example
TruthEngine.cpp
GetState(double time);

SimCamera.cpp
GenerateImages();

SimFeatureTracker.cpp
GenerateTracks();

Camera.cpp
ReadImages();

FeatureTracker.cpp
TrackFeatures();True Hardware
Integrated Modeling and Testing of ROS Nodes

ekf.cpp
PredictFilter();
UpdateMSCKF();
26

Abstraction - Example
TruthEngine.cpp
GetState(double time);

SimCamera.cpp
GenerateImages();

SimFeatureTracker.cpp
GenerateTracks();

Hardware Abstraction
Camera.cpp
ReadImages();

FeatureTracker.cpp
TrackFeatures();

Integrated Modeling and Testing of ROS Nodes

ekf.cpp
PredictFilter();
UpdateMSCKF();
27

Abstraction - Example
TruthEngine.cpp
GetState(double time);

SimCamera.cpp
GenerateImages();

SimFeatureTracker.cpp
GenerateTracks();Processor Abstraction

Camera.cpp
ReadImages();

FeatureTracker.cpp
TrackFeatures();

Integrated Modeling and Testing of ROS Nodes

ekf.cpp
PredictFilter();
UpdateMSCKF();
28

Abstraction Models: IMU

Integrated Modeling and Testing of ROS Nodes

29

Abstraction Models: Camera

Integrated Modeling and Testing of ROS Nodes

30

Abstraction - Benefits
••
•
•
•

Improves accuracy of simulation
Catches more bugs earlier
Reduces rework (no code divergence)
More beneficial unit testing
Robust Monte Carlo testing

Integrated Modeling and Testing of ROS Nodes

31

Unit Testing••••

This architecture allows test-driven development
Any tweaks or examples can become tests
Tests ensure code accuracy and functionality
Can be automated per commit / merge request

GoogleTest+

CMake+

ColconIntegrated Modeling and Testing of ROS Nodes+

lcov
32

Unit Testing Nodes• Ideally, we should test as much as possible• To unit test ROS nodes, we split the node
into a entrypoint and node classmain.cpp
int main();
Node::Node();Integrated Modeling and Testing of ROS Nodes

entrypoint.cpp
int main();node.cpp
Initialize();
DeclareParameters();
LoadParameters();
33

Unit Testing Nodes

entrypoint.cpp
int main();test.cpp
TEST_F(ExampleNode, ExampleNode_test){
ExampleNode node;
node.Initialize();node.DeclareParameters();

node.cpp
Initialize();
DeclareParameters();
LoadParameters();

node.set_parameter(rclcpp::Parameter(“param1”));
node.set_parameter(rclcpp::Parameter(“param2”));
node.LoadParameters();}

Integrated Modeling and Testing of ROS Nodes

34

Unit Testing Nodes

entrypoint.cpp
int main();test.cpp
TEST_F(ExampleNode, ExampleNode_test){
ExampleNode node;
node.Initialize();node.DeclareParameters();

node.cpp
Initialize();
DeclareParameters();
LoadParameters();

node.set_parameter(rclcpp::Parameter(“param1”));
node.set_parameter(rclcpp::Parameter(“param2”));
node.LoadParameters();}

Integrated Modeling and Testing of ROS Nodes

35

Unit Testing Nodes

entrypoint.cpp
int main();test.cpp
TEST_F(ExampleNode, ExampleNode_test){
ExampleNode node;
node.Initialize();node.DeclareParameters();

node.cpp
Initialize();
DeclareParameters();
LoadParameters();

node.set_parameter(rclcpp::Parameter(“param1”));
node.set_parameter(rclcpp::Parameter(“param2”));
node.LoadParameters();}

Integrated Modeling and Testing of ROS Nodes

36

Unit Testing Nodes

entrypoint.cpp
int main();test.cpp
TEST_F(ExampleNode, ExampleNode_test){
ExampleNode node;
node.Initialize();node.DeclareParameters();

node.cpp
Initialize();
DeclareParameters();
LoadParameters();

node.set_parameter(rclcpp::Parameter(“param1”));
node.set_parameter(rclcpp::Parameter(“param2”));
node.LoadParameters();}

Integrated Modeling and Testing of ROS Nodes

37

Unit Testing Nodes

entrypoint.cpp
int main();test.cpp
TEST_F(ExampleNode, ExampleNode_test){
ExampleNode node;
node.Initialize();node.DeclareParameters();

node.cpp
Initialize();
DeclareParameters();
LoadParameters();

node.set_parameter(rclcpp::Parameter(“param1”));
node.set_parameter(rclcpp::Parameter(“param2”));
node.LoadParameters();}

Integrated Modeling and Testing of ROS Nodes

38

Unit Testing Nodes

entrypoint.cpp
int main();test.cpp
TEST_F(ExampleNode, ExampleNode_test){
ExampleNode node;
node.Initialize();node.DeclareParameters();

node.cpp
Initialize();
DeclareParameters();
LoadParameters();

node.set_parameter(rclcpp::Parameter(“param1”));
node.set_parameter(rclcpp::Parameter(“param2”));
node.LoadParameters();}

Integrated Modeling and Testing of ROS Nodes

39

Monte Carlo Testing• With fast enough simulations, we can run thousands of example datasets• Random initialization and measurement errors are inserted• Utilizing abstractions increases confidence of filter stabilityz

Integrated Modeling and Testing of ROS Nodes

40Key Takeaways•••••

Stop developing simulations separate from deployments
Utilize existing models for integrated simulations
Abstract layers as low as possible
Utilize multiple layers of abstraction for various fidelity / execution speed
Try out EKF-CAL! We love feedback and collaboration!

Integrated Modeling and Testing of ROS Nodes

41

Presentation References
1. J. Hartzer and S. Saripalli, " Online Multi Camera-IMU Calibration", IEEE International
Symposium on Safety, Security, and Rescue Robotics (SSRR), 2022. IEEE, arXiv
2. P. Jiang and S. Saripalli, "LiDARNet: A Boundary-Aware Domain Adaptation Model for Point
Cloud Semantic Segmentation," 2021 IEEE International Conference on Robotics andAutomation (ICRA), Xi'an, China, 2021, pp. 2457-2464, IEEE
3. Experimental Evaluation of 3D-LIDAR Camera Extrinsic Calibration, S. Mishra, P. Osteen,
G. Pandey and S. Saripalli, IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS, 2020), arXiv
4. Extrinsic Calibration of a 3D-LIDAR and a Camera, S. Mishra, G. Pandey and S. Saripalli,IEEE Intelligent Vehicles Symposium (IV, 2020) arXiv
5. Chustz, G., & Saripalli, S. (2021). ROOAD: RELLIS Off-road Odometry Analysis Dataset.
arXIv
Integrated Modeling and Testing of ROS Nodes

42

Questions?

EKF-CAL Repository
Integrated Modeling and Testing of ROS Nodes

Unmanned Systems Lab43
43

44