Add rmw zenoh to nightly ci builds #5516

mini-1235 · 2025-09-08T01:36:27Z

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 · 2025-09-08T02:18:47Z

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 · 2025-09-11T08:27:38Z

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 · 2025-09-11T14:50:44Z

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 · 2025-09-11T15:26:48Z

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 · 2025-09-11T15:56:25Z

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 · 2025-09-12T04:54:19Z

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 · 2025-09-15T21:39:14Z

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

mini-1235 · 2025-09-16T05:57:10Z

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 · 2025-09-18T14:53:51Z

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

Signed-off-by: mini-1235 <[email protected]>

Signed-off-by: Maurice <[email protected]>

mini-1235 · 2025-09-27T08:08:50Z

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

SteveMacenski · 2025-09-29T19:20:16Z

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

Signed-off-by: mini-1235 <[email protected]>

mini-1235 · 2025-10-23T13:10:11Z

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 · 2025-10-23T16:20:25Z

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

Signed-off-by: mini-1235 <[email protected]>

codecov · 2025-10-23T16:44:10Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_amcl/src/amcl_node.cpp	`86.31% <100.00%> (ø)`

... and 7 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 · 2025-10-23T16:46:23Z

@SteveMacenski All tests passing now! If you agree to all my changes here, I can switch back the default to rmw_cyclone_dds

SteveMacenski · 2025-10-23T19:02:16Z

nav2_amcl/src/amcl_node.cpp

    rclcpp::CallbackGroupType::MutuallyExclusive, false);
  initParameters();
  initTransforms();
+  initOdometry();


I think this needs to be after init pubsub and init services in order to be able to have last_published_pose_ be usable

Looking at the ros(1) code, https://github.com/ros-planning/navigation/blob/f44bb1fc2810399165115cc98b530fe4b9397c18/amcl/src/amcl_node.cpp#L793-L833, I think the init_pose_ is either [0,0,0] or [initial_pose_x,y,yaw] when set_initial_pose_ is enabled

However, I cannot find the parameters initial_cov_xx, initial_cov_yy in our code. Maybe it is removed at some point in Nav2?

So, I think

navigation2/nav2_amcl/src/amcl_node.cpp

Lines 102 to 115 in 8dbc929

if (set_initial_pose_) {

auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();

msg->header.stamp = now();

msg->header.frame_id = global_frame_id_;

msg->pose.pose.position.x = initial_pose_x_;

msg->pose.pose.position.y = initial_pose_y_;

msg->pose.pose.position.z = initial_pose_z_;

msg->pose.pose.orientation = orientationAroundZAxis(initial_pose_yaw_);

initialPoseReceived(msg);

} else if (init_pose_received_on_inactive) {

handleInitialPose(last_published_pose_);

}

and
initOdometry should be called before initParticleFilter

I am not familiar with the amcl codebase, so I may have missed something. A simpler option would be initializing init_pose_ to [0,0,0] in the constructor so that there will be no garbage value when configuring amcl

SteveMacenski · 2025-10-23T19:04:05Z

.circleci/config.yml

+              # Start zenohd daemon only if using rmw_zenoh_cpp
+              if [ "$RMW_IMPLEMENTATION" = "rmw_zenoh_cpp" ]; then
+                . /opt/ros/$ROS_DISTRO/setup.sh
+                ros2 run rmw_zenoh_cpp rmw_zenohd &


Is doing this in the CI config the prevailing thought from the threads you brought up for how to handle this? I was kind of hoping for ros2 launch to deal with this so it was in the application code -- i.e. StartZenohRouter() method that we can do conditionally when the RMW implementation is set to zenoh.

... Actually, can we do that instead? Create in nav2_common that ros2 launch action and use the condition for checking that environnmental variable?

Is doing this in the CI config the prevailing thought from the threads you brought up for how to handle this?

They provide CMake functions like ament_add_ros_isolated_xxxx to help when testing using RMW Zenoh

What I was thinking is that my solution would be more straightforward, especially since manually starting the Zenoh router will not be required in the future according to their README.

If you prefer to go with ament_add_ros_isolated I can add it :)

Signed-off-by: mini-1235 <[email protected]>

mini-1235 mentioned this pull request Sep 9, 2025

Service call reports z_reply_is_ok returned False Reason: Timeout ros2/rmw_zenoh#783

Open

SteveMacenski reviewed Sep 10, 2025

View reviewed changes

mini-1235 mentioned this pull request Sep 16, 2025

Service request timeouts missing in RCL → RMW interaction ros2/rmw#404

Open

SteveMacenski added the blocked label Sep 16, 2025

mini-1235 added 3 commits September 27, 2025 14:44

Add rmw zenoh cpp to ci

850c588

Signed-off-by: mini-1235 <[email protected]>

Linting

dad4a2b

Signed-off-by: mini-1235 <[email protected]>

Sourcing ros

dc8a2ed

Signed-off-by: mini-1235 <[email protected]>

mini-1235 force-pushed the feat/zenoh branch 3 times, most recently from f60a807 to 723ac34 Compare September 27, 2025 07:49

Use spin all to avoid flaky tests

044b6ec

Signed-off-by: Maurice <[email protected]>

mini-1235 force-pushed the feat/zenoh branch from 723ac34 to 044b6ec Compare September 27, 2025 07:54

mini-1235 added 3 commits October 18, 2025 14:46

Merge remote-tracking branch 'origin/main' into feat/zenoh

3f1350b

Merge remote-tracking branch 'origin/main' into feat/zenoh

74a001b

Fix test

fe020d3

Signed-off-by: mini-1235 <[email protected]>

Fix order in AMCL

11c90c3

Signed-off-by: mini-1235 <[email protected]>

mini-1235 changed the title ~~Add rmw zenoh cpp to ci~~ Add rmw zenoh to nightly ci builds Oct 23, 2025

SteveMacenski reviewed Oct 23, 2025

View reviewed changes

mini-1235 added 3 commits October 24, 2025 15:57

Use ament_add_ros_isolated

ee4e036

Signed-off-by: mini-1235 <[email protected]>

Use ament_add_ros_isolated

6b38c05

Signed-off-by: mini-1235 <[email protected]>

retrigger ci

b3ccad1

Signed-off-by: mini-1235 <[email protected]>

	if (set_initial_pose_) {
	auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();

	msg->header.stamp = now();
	msg->header.frame_id = global_frame_id_;
	msg->pose.pose.position.x = initial_pose_x_;
	msg->pose.pose.position.y = initial_pose_y_;
	msg->pose.pose.position.z = initial_pose_z_;
	msg->pose.pose.orientation = orientationAroundZAxis(initial_pose_yaw_);

	initialPoseReceived(msg);
	} else if (init_pose_received_on_inactive) {
	handleInitialPose(last_published_pose_);
	}

Add rmw zenoh to nightly ci builds #5516

Are you sure you want to change the base?

Add rmw zenoh to nightly ci builds #5516

Conversation

mini-1235 commented Sep 8, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

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:

Uh oh!

mini-1235 commented Sep 8, 2025

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mini-1235 commented Sep 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Sep 11, 2025

Uh oh!

mini-1235 commented Sep 11, 2025

Uh oh!

SteveMacenski commented Sep 11, 2025

Uh oh!

mini-1235 commented Sep 12, 2025

Uh oh!

SteveMacenski commented Sep 15, 2025

Uh oh!

mini-1235 commented Sep 16, 2025

Uh oh!

mergify bot commented Sep 18, 2025

Uh oh!

mini-1235 commented Sep 27, 2025

Uh oh!

SteveMacenski commented Sep 29, 2025

Uh oh!

mini-1235 commented Oct 23, 2025

Uh oh!

mini-1235 commented Oct 23, 2025

Uh oh!

codecov bot commented Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mini-1235 commented Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski Oct 23, 2025

Choose a reason for hiding this comment

Uh oh!

mini-1235 Oct 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mini-1235 Oct 24, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mini-1235 Oct 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

mini-1235 commented Sep 8, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

mini-1235 commented Sep 11, 2025 •

edited

Loading

codecov bot commented Oct 23, 2025 •

edited

Loading

mini-1235 commented Oct 23, 2025 •

edited

Loading

mini-1235 Oct 24, 2025 •

edited

Loading

SteveMacenski Oct 23, 2025 •

edited

Loading

mini-1235 Oct 24, 2025 •

edited

Loading