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

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

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

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

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?

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.

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

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

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

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

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

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

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

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

When AMCL is on configure stage, it calls initParticleFilter

In the function, it uses the variable init_pose_ and init_cov_

However, this two variables are initialized later when calling initOdometry

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

I think we need to move initOdometry before initParticleFilter

Codecov Report

✅ All modified and coverable lines are covered by tests.

... and 16 files with indirect coverage changes

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

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

Otherwise LGTM!

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.

Switching back to cyclonedds now :)

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