diff --git a/.circleci/config.yml b/.circleci/config.yml index e26e29f6866..1d0c08e87df 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v21\ + - "<< parameters.key >>-v23\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v21\ + - "<< parameters.key >>-v23\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v21\ + key: "<< parameters.key >>-v23\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ @@ -82,6 +82,7 @@ _commands: - run: name: Install Dependencies | << parameters.workspace >> working_directory: << parameters.workspace >> + # Remove/Replace turtlebot3_gazebo and gazebo_ros_pkgs from --skip-keys after https://github.com/ros-navigation/navigation2/pull/3634 command: | . << parameters.underlay >>/install/setup.sh AMENT_PREFIX_PATH=$(echo "$AMENT_PREFIX_PATH" | \ @@ -96,11 +97,6 @@ _commands: (echo vcs_export && cat) >> lockfile.txt sha256sum $PWD/lockfile.txt >> lockfile.txt - # Temp: Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch - # This replaces main rosdep index with the last prior to release: Feb 28, 2024 - sed -i "s|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|" /etc/ros/rosdep/sources.list.d/20-default.list - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml - apt-get update rosdep update --rosdistro $ROS_DISTRO dependencies=$( @@ -109,6 +105,7 @@ _commands: --ignore-src \ --skip-keys " \ slam_toolbox \ + turtlebot3_gazebo \ " \ --verbose | \ awk '$1 ~ /^resolution\:/' | \ @@ -487,7 +484,7 @@ _environments: executors: release_exec: docker: - - image: ghcr.io/ros-planning/navigation2:main + - image: ghcr.io/ros-navigation/navigation2:main resource_class: large working_directory: /opt/overlay_ws environment: diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 09aaa5d31d4..77a326648db 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -3,10 +3,11 @@ "build": { "dockerfile": "../Dockerfile", "context": "..", - "target": "visualizer", - "cacheFrom": "ghcr.io/ros-planning/navigation2:main" + "target": "dever", + "cacheFrom": "ghcr.io/ros-navigation/navigation2:main" }, "runArgs": [ + "--name=nav2" // "--cap-add=SYS_PTRACE", // enable debugging, e.g. gdb // "--ipc=host", // shared memory transport with host, e.g. rviz GUIs // "--network=host", // network access to host interfaces, e.g. eth0 diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 8732b239d9b..c181fe25f4c 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -7,7 +7,7 @@ jobs: name: ament_${{ matrix.linter }} runs-on: ubuntu-latest container: - image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest strategy: fail-fast: false matrix: diff --git a/Dockerfile b/Dockerfile index 69c89230193..1f1060ccd70 100644 --- a/Dockerfile +++ b/Dockerfile @@ -57,7 +57,7 @@ RUN apt-get update && \ ros-$ROS_DISTRO-rmw-fastrtps-cpp \ ros-$ROS_DISTRO-rmw-connextdds \ ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ - && pip3 install \ + && pip3 install --break-system-packages \ fastcov \ git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \ git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \ @@ -96,11 +96,14 @@ ARG OVERLAY_WS ENV OVERLAY_WS $OVERLAY_WS WORKDIR $OVERLAY_WS COPY --from=cacher /tmp/$OVERLAY_WS ./ + +# Remove/Replace turtlebot3_gazebo and gazebo_ros_pkgs from --skip-keys after https://github.com/ros-navigation/navigation2/pull/3634 RUN . $UNDERLAY_WS/install/setup.sh && \ apt-get update && rosdep install -q -y \ --from-paths src \ --skip-keys " \ slam_toolbox \ + turtlebot3_gazebo \ "\ --ignore-src \ && rm -rf /var/lib/apt/lists/* @@ -145,7 +148,7 @@ RUN apt-get update && \ bash-completion \ gdb \ wget && \ - pip3 install \ + pip3 install --break-system-packages \ bottle \ glances diff --git a/README.md b/README.md index ab34c01ce3d..49ce011c96f 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ For detailed instructions on how to: - [Configure](https://docs.nav2.org/configuration/index.html) - [Navigation Plugins](https://docs.nav2.org/plugins/index.html) - [Migration Guides](https://docs.nav2.org/migration/index.html) -- [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2) +- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2) - [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html) Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 91fc49b71d0..6d43f4e760e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -20,6 +20,8 @@ find_package(nav2_util REQUIRED) nav2_package() +add_compile_options(-Wno-shadow) # Delete after https://github.com/BehaviorTree/BehaviorTree.CPP/issues/811 is released + include_directories( include ) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 8c9fcb54d25..c6c5b890ae6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -387,7 +387,7 @@ class BtActionNode : public BT::ActionNodeBase }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { + const std::shared_ptr feedback) { feedback_ = feedback; emitWakeUpSignal(); }; diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index fa5badf750d..c20708788c1 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -57,7 +57,7 @@ inline BT::NodeStatus GoalUpdater::tick() getInput("input_goal", goal); - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(std::chrono::milliseconds(50)); if (last_goal_received_.header.stamp != rclcpp::Time(0)) { auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); diff --git a/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp index 180e2d879e4..6fa4dfc111c 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp @@ -29,6 +29,7 @@ class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::Behavior { config_->input_ports["global_frame"] = "map"; config_->input_ports["robot_base_frame"] = "base_link"; + config_->input_ports["distance"] = 1.0; bt_node_ = std::make_shared( "distance_traveled", *config_); } @@ -89,6 +90,5 @@ int main(int argc, char ** argv) // shutdown ROS rclcpp::shutdown(); - return all_successful; } diff --git a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp index ead9dd21480..97301ed1004 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp @@ -27,6 +27,8 @@ class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::Behav public: void SetUp() { + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "globally_updated_goal", *config_); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp index 592ff5042a9..f8a856b2370 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp @@ -28,6 +28,8 @@ class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT public: void SetUp() { + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "goal_updated", *config_); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp index 7b8a67529ed..ad25521f5b8 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp @@ -26,6 +26,7 @@ class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::Behav public: void SetUp() { + config_->input_ports["initial_pose_received"] = false; bt_node_ = std::make_shared("TestNode", *config_); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 2f47ab826ad..92834a15f28 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -18,10 +18,12 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" using namespace std::chrono; // NOLINT using namespace std::chrono_literals; // NOLINT @@ -33,6 +35,9 @@ class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::Behavio { node_ = std::make_shared("test_path_expiring_condition"); config_ = new BT::NodeConfiguration(); + config_->input_ports["seconds"] = 1.0; + config_->input_ports["path"] = ""; + config_->blackboard = BT::Blackboard::create(); config_->blackboard->set("node", node_); bt_node_ = std::make_shared( diff --git a/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp index 0ec8c224e0f..4f5b8929cff 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp @@ -32,6 +32,7 @@ class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT public: void SetUp() { + config_->input_ports["seconds"] = 1.0; bt_node_ = std::make_shared( "time_expired", *config_); } diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 1ca41e798e8..82927031429 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -58,6 +58,7 @@ class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixtu public: void SetUp() override { + config_->input_ports["number_of_retries"] = 1; bt_node_ = std::make_shared( "recovery_node", *config_); first_child_ = std::make_shared(); 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 dadbec3cd4a..0fdf84b9086 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() { + config_->input_ports["distance"] = 1.0; config_->input_ports["global_frame"] = "map"; config_->input_ports["robot_base_frame"] = "base_link"; bt_node_ = std::make_shared( diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index b33e9e3a8b2..2e5d34ed4f6 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -36,7 +36,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree poses1.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); - + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "goal_updated_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp index f7095357c30..df9847e90fc 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp @@ -29,6 +29,7 @@ class RateControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFix public: void SetUp() { + config_->input_ports["hz"] = 10.0; bt_node_ = std::make_shared( "rate_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 26113b26993..903b6385ed3 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -45,6 +45,13 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi std::vector fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT + config_->input_ports["min_rate"] = 0.1; + config_->input_ports["max_rate"] = 1.0; + config_->input_ports["min_speed"] = 0.0; + config_->input_ports["max_speed"] = 0.5; + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; + bt_node_ = std::make_shared("speed_controller", *config_); dummy_node_ = std::make_shared(); bt_node_->setChild(dummy_node_.get()); diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 7080f580edb..56c8602eebf 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -19,7 +19,8 @@ navigation2 nav2_common slam_toolbox - turtlebot3_gazebo + + ament_lint_common ament_lint_auto diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 81884a6a455..40415868f54 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -1503,9 +1503,9 @@ TEST_F(Tester, testCollisionPointsMarkers) ASSERT_TRUE(waitCollisionPointsMarker(500ms)); ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); - publishCmdVel(0.5, 0.2, 0.1); publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCollisionPointsMarker(500ms)); ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); // Stop Collision Monitor node diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 6687b3452da..1ca6461e9a9 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -404,7 +404,7 @@ class SmootherTest : public ::testing::Test int cusp_i_ = -1; QualityCriterion3 mvmt_smoothness_criterion_ = [this](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_) { @@ -986,7 +986,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) int cusp_i_out = 6; // for upsampled path QualityCriterion3 mvmt_smoothness_criterion_out = [&cusp_i_out](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_out) { diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index 75de1feb917..319702c5bde 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -194,7 +194,8 @@ bool makeFootprintFromString( RCLCPP_ERROR( rclcpp::get_logger( "nav2_costmap_2d"), - "You must specify at least three points for the robot footprint, reverting to previous footprint."); //NOLINT + "You must specify at least three points for the robot footprint," + " reverting to previous footprint."); return false; } footprint.reserve(vvf.size()); @@ -209,7 +210,8 @@ bool makeFootprintFromString( RCLCPP_ERROR( rclcpp::get_logger( "nav2_costmap_2d"), - "Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", //NOLINT + "Points in the footprint specification must be pairs of numbers." + " Found a point with %d numbers.", static_cast(vvf[i].size())); return false; } diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp index b64d049ab7d..59676e23596 100644 --- a/nav2_costmap_2d/test/integration/costmap_tester.cpp +++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp @@ -129,7 +129,8 @@ void CostmapTester::compareCells( RCLCPP_ERROR( rclcpp::get_logger( "costmap_tester"), - "Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", // NOLINT + "Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost:" + " %d, cell distance: %.2f, furthest valid distance: %.2f", x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance); RCLCPP_ERROR( diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index 4d0a3bef54a..3bb257c2c6b 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -57,9 +57,10 @@ TEST(DynParamTestNode, testDynParamsSet) costmap->on_activate(rclcpp_lifecycle::State()); auto parameter_client = std::make_shared( - node->shared_from_this(), - "/test_costmap/test_costmap", - rmw_qos_profile_parameters); + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + auto results1 = parameter_client->set_parameters_atomically( { rclcpp::Parameter("robot_radius", 1.234), @@ -83,7 +84,8 @@ TEST(DynParamTestNode, testDynParamsSet) rclcpp::Parameter("robot_base_frame", "wrong_test_frame"), }); - rclcpp::spin_some(costmap->get_node_base_interface()); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + rclcpp::spin_all(costmap->get_node_base_interface(), std::chrono::milliseconds(50)); EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234); EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345); diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 0a43c136966..fd4208454bb 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) +find_package(yaml-cpp REQUIRED) nav2_package() @@ -118,7 +119,8 @@ target_include_directories(${map_io_library_name} SYSTEM PRIVATE ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) target_link_libraries(${map_io_library_name} - ${GRAPHICSMAGICKCPP_LIBRARIES}) + ${GRAPHICSMAGICKCPP_LIBRARIES} + yaml-cpp::yaml-cpp) if(WIN32) target_compile_definitions(${map_io_library_name} PRIVATE @@ -160,5 +162,5 @@ ament_export_libraries( ${library_name} ${map_io_library_name} ) -ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies}) +ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies} yaml-cpp) ament_package() diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index fae3328f066..ddde6077659 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -27,6 +27,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Warray-bounds" #pragma GCC diagnostic ignored "-Wstringop-overflow" +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #include #include #include @@ -473,7 +474,7 @@ inline void savitskyGolayFilter( auto applyFilterOverAxis = [&](xt::xtensor & sequence, - const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { unsigned int idx = 0; sequence(idx) = applyFilter( diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 4386ec1dc12..e3e49918d72 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -192,8 +192,8 @@ bool ObstaclesCritic::inCollision(float cost) const bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + using namespace nav2_costmap_2d; // NOLINT switch (static_cast(cost)) { - using namespace nav2_costmap_2d; // NOLINT case (LETHAL_OBSTACLE): return true; case (INSCRIBED_INFLATED_OBSTACLE): diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 054cbbe4c0e..3603d4f3cf3 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -15,7 +15,11 @@ #include #include +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop + #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/tools/utils.hpp" diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 2cce713b0f2..d166dbc7b1b 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -326,14 +326,14 @@ int NavFn::getPathLen() {return npath;} // inserting onto the priority blocks #define push_cur(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ - {curP[curPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ + {curP[curPe++] = n; pending[n] = true;}} #define push_next(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ - {nextP[nextPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ + {nextP[nextPe++] = n; pending[n] = true;}} #define push_over(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ - {overP[overPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ + {overP[overPe++] = n; pending[n] = true;}} // Set up navigation potential arrays for new propagation diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index b1e265e139b..9dec3ffa7ef 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -463,6 +463,16 @@ class NodeHybrid */ bool backtracePath(CoordinateVector & path); + /** + * @brief Destroy shared pointer assets at the end of the process that don't + * require normal destruction handling + */ + static void destroyStaticAssets() + { + inflation_layer.reset(); + costmap_ros.reset(); + } + NodeHybrid * parent; Coordinates pose; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 0e0c8829222..36201b26de4 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -319,6 +319,7 @@ void SmacPlannerHybrid::cleanup() RCLCPP_INFO( _logger, "Cleaning up plugin %s of type SmacPlannerHybrid", _name.c_str()); + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); _a_star.reset(); _smoother.reset(); if (_costmap_downsampler) { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 4f2dba45e43..10605094e45 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -258,6 +258,7 @@ void SmacPlannerLattice::cleanup() RCLCPP_INFO( _logger, "Cleaning up plugin %s of type SmacPlannerLattice", _name.c_str()); + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); _a_star.reset(); _smoother.reset(); _raw_plan_publisher.reset(); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index bdcf37c7215..3ec5156f23a 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -215,6 +215,7 @@ TEST(AStarTest, test_a_star_se2) EXPECT_GT(expansions->size(), 5u); delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(AStarTest, test_a_star_lattice) @@ -290,6 +291,7 @@ TEST(AStarTest, test_a_star_lattice) } delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(AStarTest, test_se2_single_pose_path) @@ -348,6 +350,7 @@ TEST(AStarTest, test_se2_single_pose_path) EXPECT_GE(path.size(), 1u); delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(AStarTest, test_constants) diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 1326fb02136..7b99858500f 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -145,7 +145,7 @@ TEST(Node2DTest, test_node_2d_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 856f37c0a11..fb17dad5201 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -236,6 +236,7 @@ TEST(NodeHybridTest, test_obstacle_heuristic) EXPECT_EQ(wide_passage_cost, two_passages_cost); delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(NodeHybridTest, test_node_debin_neighbors) @@ -364,7 +365,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 2355ac9a842..e118388a073 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -306,7 +306,7 @@ TEST(NodeLatticeTest, test_get_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 616c674595a..3ef261f8a51 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -189,4 +189,5 @@ TEST(SmootherTest, test_full_smoother) EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3); delete costmap; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index bfab0a79fed..45a8097b355 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(nav2_system_tests) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 23) +endif() + find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) @@ -14,7 +18,7 @@ find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(gazebo_ros_pkgs REQUIRED) +# find_package(gazebo_ros_pkgs REQUIRED) Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 find_package(nav2_amcl REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(rclpy REQUIRED) @@ -37,7 +41,7 @@ set(dependencies nav2_amcl nav2_lifecycle_manager nav2_behavior_tree - gazebo_ros_pkgs +# gazebo_ros_pkgs Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 geometry_msgs std_msgs tf2_geometry_msgs @@ -109,18 +113,19 @@ if(BUILD_TESTING) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) - add_subdirectory(src/localization) - add_subdirectory(src/system) - add_subdirectory(src/system_failure) - add_subdirectory(src/updown) - add_subdirectory(src/waypoint_follower) - add_subdirectory(src/gps_navigation) - add_subdirectory(src/behaviors/spin) - add_subdirectory(src/behaviors/wait) - add_subdirectory(src/behaviors/backup) - add_subdirectory(src/behaviors/drive_on_heading) - add_subdirectory(src/behaviors/assisted_teleop) - add_subdirectory(src/costmap_filters) + # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 + # add_subdirectory(src/localization) + # add_subdirectory(src/system) + # add_subdirectory(src/system_failure) + # add_subdirectory(src/updown) + # add_subdirectory(src/waypoint_follower) + # add_subdirectory(src/gps_navigation) + # add_subdirectory(src/behaviors/spin) + # add_subdirectory(src/behaviors/wait) + # add_subdirectory(src/behaviors/backup) + # add_subdirectory(src/behaviors/drive_on_heading) + # add_subdirectory(src/behaviors/assisted_teleop) + # add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 049b2b2181d..5fdbc230b73 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -26,7 +26,7 @@ geometry_msgs std_msgs tf2_geometry_msgs - gazebo_ros_pkgs + launch_ros launch_testing nav2_planner @@ -48,7 +48,7 @@ nav2_amcl std_msgs tf2_geometry_msgs - gazebo_ros_pkgs + navigation2 lcov robot_state_publisher diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 0a4b320d8f4..100e5dad936 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -79,10 +79,10 @@ class BehaviorTreeHandler return false; } - auto xml_string = std::string( - std::istreambuf_iterator(xml_file), - std::istreambuf_iterator()); - + std::stringstream buffer; + buffer << xml_file.rdbuf(); + xml_file.close(); + std::string xml_string = buffer.str(); // Create the blackboard that will be shared by all of the nodes in the tree blackboard = BT::Blackboard::create(); diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 3af965d4c68..5ecd95d60b9 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -81,6 +81,13 @@ class FibonacciServerNode : public rclcpp::Node // assert(action_server_->is_cancel_requested() == false); // auto feedback = std::make_shared(); // action_server_->publish_feedback(feedback); + omit_preempt_subs_.reset(); + activate_subs_.reset(); + deactivate_subs_.reset(); + while (action_server_->is_running()) { + std::this_thread::sleep_for(10ms); + } + action_server_->deactivate(); action_server_.reset(); } @@ -150,21 +157,33 @@ class RclCppFixture std::make_shared(std::bind(&RclCppFixture::server_thread_func, this)); } + void TearDown() + { + stop_.store(true); + if (server_thread_ && server_thread_->joinable()) { + server_thread_->join(); + server_thread_.reset(); + } + } + ~RclCppFixture() { - server_thread_->join(); } void server_thread_func() { auto node = std::make_shared(); node->on_init(); - rclcpp::spin(node->get_node_base_interface()); + while (rclcpp::ok() && !stop_.load()) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } node->on_term(); node.reset(); } std::shared_ptr server_thread_; + std::atomic stop_{false}; }; RclCppFixture g_rclcppfixture; @@ -189,6 +208,9 @@ class ActionTestNode : public rclcpp::Node void on_term() { + omit_prempt_pub_.reset(); + activate_pub_.reset(); + deactivate_pub_.reset(); action_client_.reset(); } @@ -547,6 +569,7 @@ int main(int argc, char ** argv) g_rclcppfixture.Setup(); ::testing::InitGoogleTest(&argc, argv); auto result = RUN_ALL_TESTS(); + g_rclcppfixture.TearDown(); rclcpp::shutdown(); return result; } diff --git a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp index 345db77d857..82ebf74ec09 100644 --- a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp +++ b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp @@ -42,6 +42,18 @@ class WPShim : public nav2_waypoint_follower::WaypointFollower rclcpp_lifecycle::State state; this->on_activate(state); } + + void deactivate() + { + rclcpp_lifecycle::State state; + this->on_deactivate(state); + } + + void cleanup() + { + rclcpp_lifecycle::State state; + this->on_cleanup(state); + } }; class RclCppFixture @@ -73,4 +85,7 @@ TEST(WPTest, test_dynamic_parameters) EXPECT_EQ(follower->get_parameter("loop_rate").as_int(), 100); EXPECT_EQ(follower->get_parameter("stop_on_failure").as_bool(), false); + follower->deactivate(); + follower->cleanup(); + follower.reset(); } diff --git a/tools/underlay.repos b/tools/underlay.repos index dd12e220027..a0f6a83ae27 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -35,10 +35,6 @@ repositories: # type: git # url: https://github.com/cra-ros-pkg/robot_localization.git # version: ros2 - # ros-simulation/gazebo_ros_pkgs: - # type: git - # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git - # version: ros2 # ros2/geometry2: # type: git # url: https://github.com/ros2/geometry2.git