Add rmw zenoh to nightly ci builds

[mini-1235]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	closes #5509
Primary OS tested on	Ubuntu
Robotic platform tested on
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

• Add zenoh to ci matrix
• Fix tests to pass with zenoh

Description of documentation updates required from your changes

Description of how this change was tested

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[mini-1235]

Two of the tests are still failing, this PR is not ready for review

[SteveMacenski]

Please leave the default as is before merging, but I understand it’s useful to test in CI for now.

Also, why did the docking test need to be updated? Did you find root cause or is that a workaround for zenoh to work? If a workaround, that might indicate a problem in the RMW if there’s not a logical error in the test.

[mini-1235]

Please leave the default as is before merging, but I understand it’s useful to test in CI for now.

Will do after all tests pass

Also, why did the docking test need to be updated? Did you find root cause or is that a workaround for zenoh to work? If a workaround, that might indicate a problem in the RMW if there’s not a logical error in the test.

I will explain the changes I made to the test here:

• test_distance_controller: originally reporting Input t_sec is too large or too small for tf2::Duration, the cause is due to missing declare_parameter for transform tolerance

• test_astar: thread panicked, related to https://github.com/ros2/rmw_zenoh?tab=readme-ov-file#crash-when-program-terminates

• test_controller: flaky, it fails when the subscriber does not receive the latest costmap message published here

navigation2/nav2_docking/opennav_docking/test/test_controller.cpp

Line 514 in 8aea1a8

collision_tester->publishCostmap();

I can actually reproduce this as well in cyclonedds when running 1000 times repeatedly

• test_bt_navigator_with_wrong_init_pose (still failing): this is because the initial_transform_timeout and rmw zenoh's queries_default_timeout are both 60s by default. If we set the timeout to > 60s in service client, and it actually takes > 60s for the service server, then it will fail, see more in Service call reports z_reply_is_ok returned False Reason: Timeout ros2/rmw_zenoh#783

We can actually decrease initial_transform_timeout / increase queries_default_timeout and overwrite the default zenoh config, but I am not sure if these are the best solutions

[SteveMacenski]

Ok sounds good to me. I might prefer spinning for those 10ms rather than having a background spin & 10ms sleeps, but I generally don’t nitpick over tests. Are you working on the last failure in CI? The rest LGTM

[mini-1235]

Are you working on the last failure in CI?

Yes, it looks like that we need to overwrite the zenoh config at least for the CI ros2/rmw_zenoh#783 (comment), what do you think about that?

[SteveMacenski]

It seems to me like something that should be addressed in the RMW / Zenoh layers. If this is a quirk of ROS 2, then either that should change, RMW Zenoh should adjust, or the default configurations should handle it.

[mini-1235]

It seems to me like something that should be addressed in the RMW / Zenoh layers. If this is a quirk of ROS 2, then either that should change, RMW Zenoh should adjust, or the default configurations should handle it.

I share the same opinion. Admittedly, I am not a zenoh expert, so I think I might need to leave this PR open until we can find a solution together with the zenoh maintainers

[SteveMacenski]

OK - did you open a ticket on this with the RMW? Might be good and then link this PR

[mini-1235]

OK - did you open a ticket on this with the RMW? Might be good and then link this PR

Good idea, I have open a new issue in the rmw repo

[mergify]

This pull request is in conflict. Could you fix it @mini-1235?

[mini-1235]

@SteveMacenski, I have just rebased this, now waiting for next rmw zenoh's release to pass all tests

[SteveMacenski]

OK! Ping me when we can take a look again!

[mini-1235]

I might prefer spinning for those 10ms rather than having a background spin & 10ms sleeps

I tried using executor.spin_all(std::chrono::milliseconds(100));, but the tests are still flaky. Running the executor in background and sleeps resolve the issue across all RMW implementations.

[amcl-6] *** stack smashing detected *: terminated
[amcl-6] Stack trace (most recent call last):
[amcl-6] #26 Object "", at 0xffffffffffffffff, in
[amcl-6] #25 Object "/opt/overlay_ws/build/nav2_amcl/amcl", at 0x55c088dcfea4, in _start
[amcl-6] #24 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e4859d28a, in _libc_start_main
[amcl-6] #23 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e4859d1c9, in
[amcl-6] #22 Object "/opt/overlay_ws/build/nav2_amcl/amcl", at 0x55c088dd118d, in main
[amcl-6] #21 Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f6e48b5ebb8, in rclcpp::spin(std::shared_ptrrclcpp::node_interfaces::NodeBaseInterface)
[amcl-6] #20 Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f6e48b65ae3, in rclcpp::executors::SingleThreadedExecutor::spin()
[amcl-6] #19 Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f6e48b54c19, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[amcl-6] #18 Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f6e48b50483, in rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)
[amcl-6] #17 Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f6e48c23f39, in
[amcl-6] #16 Object "/opt/ros/rolling/lib/librclcpp_lifecycle.so", at 0x7f6e48cc8d34, in
[amcl-6] #15 Object "/opt/ros/rolling/lib/librclcpp_lifecycle.so", at 0x7f6e48cc28ab, in std::Function_handler<void (std::shared_ptr<rmw_request_id_s>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request<std::allocator > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response<std::allocator > >), std::_Bind<void (rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::(rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl, std::Placeholder<1>, std::Placeholder<2>, std::Placeholder<3>))(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request<std::allocator > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response<std::allocator > >)> >::M_invoke(std::Any_data const&, std::shared_ptr<rmw_request_id_s>&&, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request<std::allocator > >&&, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response<std::allocator > >&&)
[amcl-6] #14 Object "/opt/ros/rolling/lib/librclcpp_lifecycle.so", at 0x7f6e48cc1914, in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request<std::allocator > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator > >)
[amcl-6] #13 Object "/opt/ros/rolling/lib/librclcpp_lifecycle.so", at 0x7f6e48cc0d39, in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::change_state(unsigned char, rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn&)
[amcl-6] #12 Object "/opt/ros/rolling/lib/librclcpp_lifecycle.so", at 0x7f6e48cc09fe, in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) const
[amcl-6] #11 Object "/opt/overlay_ws/install/nav2_amcl/lib/libamcl_core.so", at 0x7f6e48ea3ba4, in nav2_amcl::AmclNode::on_configure(rclcpp_lifecycle::State const&)
[amcl-6] #10 Object "/opt/overlay_ws/install/nav2_amcl/lib/libamcl_core.so", at 0x7f6e48e85b29, in nav2_amcl::AmclNode::initParticleFilter()
[amcl-6] #9 Object "/opt/overlay_ws/install/nav2_amcl/lib/libpf_lib.so", at 0x7f6e4811b7de, in pf_init
[amcl-6] #8 Object "/opt/overlay_ws/install/nav2_amcl/lib/libpf_lib.so", at 0x7f6e4811c98c, in pf_pdf_gaussian_alloc
[amcl-6] #7 Object "/opt/overlay_ws/install/nav2_amcl/lib/libpf_lib.so", at 0x7f6e4811cf7b, in pf_matrix_unitary
[amcl-6] #6 Object "/opt/overlay_ws/install/nav2_amcl/lib/libpf_lib.so", at 0x7f6e4811e243, in eigen_decomposition
[amcl-6] #5 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e486aaed3, in __stack_chk_fail
[amcl-6] #4 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e486a9c48, in __fortify_fail
[amcl-6] #3 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e4859c7b5, in
[amcl-6] #2 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e4859b8fe, in abort
[amcl-6] #1 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e485b827d, in raise
[amcl-6] #0 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f6e48611b2c, in pthread_kill
[amcl-6] Aborted (Signal sent by tkill() 2450 0)

New test failing, and this is exactly the same error reported in #5470, I will try to reproduce it locally

[mini-1235]

I cannot reproduce this locally, but I think I understand the cause.

When AMCL is on configure stage, it calls initParticleFilter

navigation2/nav2_amcl/src/amcl_node.cpp

Line 75 in 47327a5

initParticleFilter();

In the function, it uses the variable init_pose_ and init_cov_

navigation2/nav2_amcl/src/amcl_node.cpp

Lines 1453 to 1460 in 47327a5

	pf_init_pose_mean.v[0] = init_pose_[0];
	pf_init_pose_mean.v[1] = init_pose_[1];
	pf_init_pose_mean.v[2] = init_pose_[2];
	pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
	pf_init_pose_cov.m[0][0] = init_cov_[0];
	pf_init_pose_cov.m[1][1] = init_cov_[1];
	pf_init_pose_cov.m[2][2] = init_cov_[2];

However, this two variables are initialized later when calling initOdometry

navigation2/nav2_amcl/src/amcl_node.cpp

Line 80 in 47327a5

initOdometry();

That's probably why we are seeing NaNs here #5470 (comment)

I think we need to move initOdometry before initParticleFilter

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_amcl/src/amcl_node.cpp	86.49% <100.00%> (+0.12%)	⬆️

... and 16 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[mini-1235]

I did some modifications:

• For CI, it always build with isolated test
• For users, it only build with isolated test when specified with --cmake-args -DUSE_ISOLATED_TESTS=ON

[SteveMacenski]

For users, it only build with isolated test when specified with --cmake-args -DUSE_ISOLATED_TESTS=ON

I think then we either need to have something that checks for Zenoh set on compilation so we do this for Zenoh automatically or we update the Development Guide / Migration Guide to mention the fact that if you use Zenoh this is required to run the Router for tests to pass

[SteveMacenski]

Otherwise LGTM!

[mini-1235]

I think then we either need to have something that checks for Zenoh set on compilation so we do this for Zenoh automatically or we update the Development Guide / Migration Guide to mention the fact that if you use Zenoh this is required to run the Router for tests to pass

I don't think this is necessary. From my experience, I usually start the Zenoh router in a separate terminal and build without isolated tests. If we run colcon test without starting the router and without building with isolated options, we will still see warnings such as:

2025-11-23T10:27:24.208659Z  WARN ThreadId(06) zenoh::net::runtime::orchestrator: Scouting delay elapsed before start conditions are met.
[WARN 1763893644.208794014] [rmw_zenoh_cpp]: Unable to connect to a Zenoh router. Have you started a router with `ros2 run rmw_zenoh_cpp rmw_zenohd`? (Data() at ./src/detail/rmw_context_impl_s.cpp:111)
[WARN 1763893644.208840326] [rmw_zenoh_cpp]: Unable to connect to a Zenoh router after 1 attempt(s). Please ensure that a Zenoh router is running and can be reached. You may increase the number of attempts to check for a router by setting the ZENOH_ROUTER_CHECK_ATTEMPTS environment variable. Proceeding with initialization but other peers will not discover or receive data from peers in this session until a router is started. (Data() at ./src/detail/rmw_context_impl_s.cpp:111)

This aligns with normal Zenoh usage and is not specific to tests. However, if you prefer, I can also add a note in the migration guide mentioning that building with isolated options is also possible.

[mini-1235]

Switching back to cyclonedds now :)

[SteveMacenski]

However, if you prefer, I can also add a note in the migration guide mentioning that building with isolated options is also possible.

Please - I think things like that can save some poor new developer a solid few hours if we leave a breadcrumb for them