Add rmw zenoh cpp to ci

.circleci/config.yml

@@ -234,6 +234,11 @@ _commands:
             name: Test Workspace | << parameters.workspace >>
             working_directory: << parameters.workspace >>
             command: |
+              # 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 &
+              fi
               TEST_UNPASSED=$(
                 colcon list \
                   --names-only \
@@ -503,7 +508,7 @@ _jobs:
         type: boolean
         default: false
       rmw:
-        default: "rmw_cyclonedds_cpp"
+        default: "rmw_zenoh_cpp"
         type: string
     parallelism: 1
     environment:
@@ -564,6 +569,7 @@ workflows:
               rmw:
                 - rmw_cyclonedds_cpp
                 - rmw_fastrtps_cpp
+                - rmw_zenoh_cpp
     triggers:
       - schedule:
           cron: "0 13 * * *"

Dockerfile

@@ -57,6 +57,7 @@ RUN apt-get update && \
       ros-$ROS_DISTRO-rmw-fastrtps-cpp \
       ros-$ROS_DISTRO-rmw-connextdds \
       ros-$ROS_DISTRO-rmw-cyclonedds-cpp \
+      ros-$ROS_DISTRO-rmw-zenoh-cpp \
     && pip3 install --break-system-packages \
       fastcov \
       git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \

nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

@@ -30,6 +30,7 @@ class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTes
 public:
   void SetUp()
   {
+    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1});
     config_->input_ports["distance"] = 1.0;
     config_->input_ports["global_frame"] = "map";
     config_->input_ports["robot_base_frame"] = "base_link";

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -304,23 +304,23 @@ TEST(ControllerTests, CollisionCheckerDockForward) {
   // Publish an empty costmap
   // It should not hit anything in an empty costmap
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
 
   // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot
   // It should hit the dock because the robot is 0.5m wide and the dock pose is at 1.75
   // But it does not hit because the collision tolerance is 0.3m
   collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
 
   // Set an object between the robot and the dock
   // It should hit the object
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 0.2, 1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
 
   // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
@@ -331,7 +331,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) {
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false));
 
   collision_tester->deactivate();
@@ -371,23 +371,23 @@ TEST(ControllerTests, CollisionCheckerDockBackward) {
   // Publish an empty costmap
   // It should not hit anything in an empty costmap
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
 
   // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot
   // It should hit the dock because the robot is 0.5m wide and the dock pose is at -1.75
   // But it does not hit because the collision tolerance is 0.3m
   collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
 
   // Set an object between the robot and the dock
   // It should hit the object
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 0.2, -1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
 
   // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
@@ -398,7 +398,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) {
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true));
 
   collision_tester->deactivate();
@@ -438,31 +438,31 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) {
   // Publish an empty costmap
   // It should not hit anything in an empty costmap
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
 
   // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked
   // It should hit the dock because the robot is 0.5m wide and the robot pose is at 1.75
   // But it does not hit because the collision tolerance is 0.3m
   collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
 
   // Set an object beyond the staging pose
   // It should hit the object
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 0.2, -1.75 - 0.5, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
 
   // Set an object between the robot and the staging pose
   // It should hit the object
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 0.2, -1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
 
   // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
@@ -473,7 +473,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) {
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));
 
   collision_tester->deactivate();
@@ -513,30 +513,30 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
   // Publish an empty costmap
   // It should not hit anything in an empty costmap
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
 
   // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked
   // It should not hit anything because the robot is docked and the trajectory is backward
   collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
 
   // Set an object beyond the staging pose
   // It should hit the object
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 0.3, 1.75 + 0.5, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
 
   // Set an object between the robot and the staging pose
   // It should hit the object
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 0.2, 1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
 
   // Set the collision tolerance to 0 to ensure all obstacles in the path are detected
@@ -547,7 +547,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
   collision_tester->clearCostmap();
   collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
   collision_tester->publishCostmap();
-  executor.spin_some();
+  executor.spin_all(std::chrono::milliseconds(50));
   EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));
 
   collision_tester->deactivate();

nav2_smac_planner/test/test_a_star.cpp

@@ -526,6 +526,8 @@ TEST(AStarTest, test_goal_heading_mode)
     a_star.setGoal(
       80u, 80u, 10u,
       nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error);
+  delete costmapA;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
 TEST(AStarTest, test_constants)