diff --git a/.circleci/config.yml b/.circleci/config.yml index 841d60042d1..69ea730b1f6 100644 --- a/.circleci/config.yml +++ b/.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 * * *" diff --git a/Dockerfile b/Dockerfile index fb3555eb8fb..0d6a84e723a 100644 --- a/Dockerfile +++ b/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 \ diff --git a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp index dcbf37838be..93fcb0d3902 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp +++ b/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"; diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index e8de87d99c6..208a4afb652 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -304,7 +304,7 @@ 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 @@ -312,7 +312,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { // 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 @@ -320,7 +320,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) { 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,7 +371,7 @@ 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 @@ -379,7 +379,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { // 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 @@ -387,7 +387,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { 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,7 +438,7 @@ 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 @@ -446,7 +446,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { // 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 @@ -454,7 +454,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { 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 @@ -462,7 +462,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { 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,14 +513,14 @@ 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 @@ -528,7 +528,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { 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 @@ -536,7 +536,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { 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(); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 751e1f2f6c8..79aaee90c96 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/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)