I kept running into the same thing. You’ve got a planner that works, you write a new one, and the only way to really know if it’s better is real data. But real data means the real robot, which means handing control to something you don’t trust yet. So the new version sits around untested, or you convince yourself sim was good enough.
So I wrote ros2_shadow. Both nodes run at once on the same inputs, only production actually controls anything, and the tool sits in the middle logging where the two would have done something different.
Output looks like this:
ros2_shadow /shadow/planner/path vs /planner/path
matched 27 unmatched prod 0 unmatched cand 0 pending 0
paired on header.stamp
metric mean p95 max
endpoint_distance 0.2663 0.3900 0.3900
hausdorff_distance 1.7351 4.7955 4.7955
length_difference 0.4506 1.1071 1.1071
warnings 14 critical 13
On keeping the candidate away from hardware. My first instinct was namespace remapping plus a denylist of topics it isn’t allowed to publish on. That doesn’t actually work. Remapping only applies to names the node declares through the API, so anything built at runtime slips through, and services and actions aren’t covered at all. And a graph watcher only tells you about a breach after the message has gone out, which is too late to be worth much.
So the candidate gets its own ROS_DOMAIN_ID instead. The hardware topics simply don’t exist over there. A bridge process holds two rclpy contexts and moves only the topics you list, and it prints exactly what crosses before it starts. I tested it with a candidate hammering /cmd_vel at 20 Hz: 0 messages made it into the production domain, 161 in its own.
Worth being clear that this only covers ROS-level access. If your candidate opens a serial port, you want a container.
The matching turned out to be the actual hard part. Both nodes see the same input but finish at different times, so you can’t just compare the latest message from each. I buffer both sides and pair within a tolerance.
The annoying bit is which timestamp to pair on. header.stamp is the right answer when it’s there, but plenty of nodes don’t copy the input’s stamp onto the output. Nav2’s planner stamps its path with now(), so the field you want to join on just isn’t populated with anything useful. Falls back to arrival time in that case, and the tool tells you which clock it used, because a pairing based on arrival is a much weaker claim than one based on the stamp of the input that produced both.
Metrics are per message type. A generic diff isn’t much use:
Type Metrics
Twist linear/angular error, direction_reversal
PoseStamped translation, rotation error
JointTrajectory per-joint RMSE, max delta, endpoint, duration
Path Hausdorff, mean deviation, endpoint, length difference
direction_reversal is the one I’d point at. If production says +0.4 and the candidate says -0.3, an error norm calls that 0.7 and buries it next to ordinary drift. It’s not drift, the robot is going backwards.
Paths get several numbers for a similar reason. Big Hausdorff with a small endpoint distance means it went a different way to the same place, which is usually fine. Small Hausdorff with a big endpoint distance means it followed you and then stopped somewhere else, which usually isn’t. One number can’t tell you which happened.
Demo is two Nav2 planner servers on one map with the same costmap config, NavFn as production and Smac 2D as the candidate, same goals to both. No simulator, the probe just hands them start poses.
ros2 launch ros2_shadow nav2_shadow_demo.launch.py
ros2 run ros2_shadow shadow config/nav2_shadow.yaml
rviz2 -d rviz/nav2_shadow.rviz
They end up routing about 4.8 m apart while finishing within 0.39 m of each other, which is the “different route, same destination” case above.
Mostly I’d like to know which message types people would need that aren’t there, and whether the domain isolation approach survives contact with how you actually deploy.
