diff --git a/.github/deps.repos b/.github/deps.repos index 1fe01fad..399f5830 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -3,3 +3,8 @@ repositories: type: git url: https://github.com/Fields2Cover/Fields2Cover.git version: v1.2.1-devel + + navigation2: + type: git + url: https://github.com/ros-navigation/navigation2.git + version: main diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index b20206a2..d89f2da8 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-noble-ros-jazzy-ros-base-latest + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-rolling-ros-base:master strategy: fail-fast: false matrix: @@ -17,7 +17,7 @@ jobs: - uses: ros-tooling/action-ros-lint@v0.1 with: linter: ${{ matrix.linter }} - distribution: jazzy + distribution: rolling package-name: | opennav_coverage opennav_coverage_bt diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e52fdb96..1a70dc12 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -12,19 +12,20 @@ jobs: ROS_DISTRO: ${{ matrix.ros_distro }} RMW_IMPLEMENTATION: rmw_cyclonedds_cpp container: - image: rostooling/setup-ros-docker:ubuntu-noble-latest + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-rolling-ros-base:master strategy: fail-fast: false matrix: - ros_distro: [jazzy] + ros_distro: [rolling] steps: - uses: actions/checkout@v2 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Build and run tests id: action-ros-ci - uses: ros-tooling/action-ros-ci@v0.3 + uses: ros-tooling/action-ros-ci@v0.4 with: + package-name: nav2_bt_navigator nav2_behavior_tree opennav_coverage opennav_coverage_bt opennav_coverage_demo opennav_coverage_msgs opennav_coverage_navigator opennav_row_coverage import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" diff --git a/README.md b/README.md index ca0b40a9..c92477bf 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ This server exposes all of the features of Fields2Cover as a Lifecycle-Component - `opennav_coverage_navigator`: Contains the BT Navigator plugin exposing `NavigateCompleteCoverage` action server analog to `NavigateToPose` and `NavigateThroughPoses`. -- `opennav_coverage_demo`: Contains an example launch and python node to trigger the coverage planner for a demonstration of the Navigator Plugin, Server, BT nodes, XML, and such. +- `opennav_coverage_demo`: Contains an example launch and python node to trigger the coverage planner for a demonstration of the Navigator Plugin, Server, BT nodes, XML, and such. Fields2Cover is a living library with new features planned to be added (for example those discussed in a [Nav2 integration ticket](https://github.com/Fields2Cover/Fields2Cover/issues/73)). As such, as new F2C capabilities are created, they will be welcome for integration here. If you see anything missing, please let us know or submit a PR to expose it! @@ -61,7 +61,7 @@ The result returns a `result.nav_path` -- which is a `nav_msgs/Path` containing It also returns an error code, if any error occurred and the total planning time for metrics analysis. -Note that `SwathMode` settings are to be paired with the `opennav_coverage` server with polygons, while the `RowSwathMode` settings are to be paired with the `opennav_row_coverage` server based on annotated rows. +Note that `SwathMode` settings are to be paired with the `opennav_coverage` server with polygons, while the `RowSwathMode` settings are to be paired with the `opennav_row_coverage` server based on annotated rows. ### NavigateCompleteCoverage @@ -82,7 +82,6 @@ The complete set of options are exposed as both dynamic parameters and through t | Parameter | Description | Type | |------------------------------|------------------------------------------------|--------| -| action_server_result_timeout | Action server result holding timeout (s) | double | | coordinates_in_cartesian_frame | Whether incoming requests are in cartesian or GPS coordinates | bool | | robot_width | Robots width (m) | double | | operation_width | Width of implement or task for coverage | double | @@ -146,7 +145,7 @@ N/A #### A Quick Note On Skipping -Rows(r) and swaths(s) are numbered as such. r1, s1, r2, s2, .... rN-1, sN-1, rN. +Rows(r) and swaths(s) are numbered as such. r1, s1, r2, s2, .... rN-1, sN-1, rN. In order to skip particular rows the 'opennav_coverage_msgs/RowSwathMode' provides 'skip_ids' to be populated. For example, if the skip ids were set to {1, 3} for five rows the output would be the following: s2, s4. @@ -156,7 +155,7 @@ As noted about `opennav_row_coverage` provides three ways to compute swaths whic The `CENTER` generator iterates through each row, calculating a center between consecutive rows. -The `OFFSET` generator iterates through each row, calculating a relative offset between consecutive rows. +The `OFFSET` generator iterates through each row, calculating a relative offset between consecutive rows. The `SWATHSAREROWS` generator iterates through each row and uses that row as the swath. diff --git a/opennav_coverage/include/opennav_coverage/coverage_server.hpp b/opennav_coverage/include/opennav_coverage/coverage_server.hpp index ccb9ec71..54225189 100644 --- a/opennav_coverage/include/opennav_coverage/coverage_server.hpp +++ b/opennav_coverage/include/opennav_coverage/coverage_server.hpp @@ -21,9 +21,9 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_util/simple_action_server.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/simple_action_server.hpp" #include "opennav_coverage/headland_generator.hpp" #include "opennav_coverage/swath_generator.hpp" #include "opennav_coverage/route_generator.hpp" @@ -37,10 +37,10 @@ namespace opennav_coverage * @brief An action server which implements highly reconfigurable complete * coverage planning using the Fields2Cover library */ -class CoverageServer : public nav2_util::LifecycleNode +class CoverageServer : public nav2::LifecycleNode { public: - using ActionServer = nav2_util::SimpleActionServer; + using ActionServer = nav2::SimpleActionServer; /** * @brief A constructor for opennav_coverage::CoverageServer @@ -79,35 +79,35 @@ class CoverageServer : public nav2_util::LifecycleNode * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; /** * @brief Activate member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; /** * @brief Deactivate member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; /** * @brief Reset member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; /** * @brief Called when in shutdown state * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; /** * @brief Callback executed when a parameter change is detected @@ -120,7 +120,7 @@ class CoverageServer : public nav2_util::LifecycleNode rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; - std::unique_ptr action_server_; + typename ActionServer::SharedPtr action_server_; std::unique_ptr robot_params_; std::unique_ptr headland_gen_; diff --git a/opennav_coverage/include/opennav_coverage/headland_generator.hpp b/opennav_coverage/include/opennav_coverage/headland_generator.hpp index 99eaef0e..7e7f0eeb 100644 --- a/opennav_coverage/include/opennav_coverage/headland_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/headland_generator.hpp @@ -21,8 +21,8 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "opennav_coverage_msgs/msg/headland_mode.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_coverage/types.hpp" @@ -45,13 +45,13 @@ class HeadlandGenerator { logger_ = node->get_logger(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_headland_type", rclcpp::ParameterValue("CONSTANT")); std::string type_str = node->get_parameter("default_headland_type").as_string(); default_type_ = toType(type_str); default_generator_ = createGenerator(default_type_); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_headland_width", rclcpp::ParameterValue(2.0)); default_headland_width_ = node->get_parameter("default_headland_width").as_double(); } diff --git a/opennav_coverage/include/opennav_coverage/path_generator.hpp b/opennav_coverage/include/opennav_coverage/path_generator.hpp index aa70984e..b6d1febf 100644 --- a/opennav_coverage/include/opennav_coverage/path_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/path_generator.hpp @@ -22,8 +22,8 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "opennav_coverage_msgs/msg/path_mode.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_coverage/types.hpp" @@ -48,17 +48,17 @@ class PathGenerator logger_ = node->get_logger(); robot_params_ = robot_params; - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_path_type", rclcpp::ParameterValue("DUBIN")); std::string type_str = node->get_parameter("default_path_type").as_string(); default_type_ = toType(type_str); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_path_continuity_type", rclcpp::ParameterValue("CONTINUOUS")); std::string type_cont_str = node->get_parameter("default_path_continuity_type").as_string(); default_continuity_type_ = toContinuityType(type_cont_str); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_turn_point_distance", rclcpp::ParameterValue(0.1)); default_turn_point_distance_ = node->get_parameter("default_turn_point_distance").as_double(); diff --git a/opennav_coverage/include/opennav_coverage/robot_params.hpp b/opennav_coverage/include/opennav_coverage/robot_params.hpp index 35ba0a2a..1af5958e 100644 --- a/opennav_coverage/include/opennav_coverage/robot_params.hpp +++ b/opennav_coverage/include/opennav_coverage/robot_params.hpp @@ -21,8 +21,8 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_coverage/types.hpp" @@ -42,19 +42,19 @@ class RobotParams template explicit RobotParams(const NodeT & node) { - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "robot_width", rclcpp::ParameterValue(2.1)); robot_.robot_width = node->get_parameter("robot_width").as_double(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "operation_width", rclcpp::ParameterValue(2.5)); robot_.op_width = node->get_parameter("operation_width").as_double(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "min_turning_radius", rclcpp::ParameterValue(0.4)); robot_.setMinRadius(node->get_parameter("min_turning_radius").as_double()); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "linear_curv_change", rclcpp::ParameterValue(2.0)); robot_.linear_curv_change = node->get_parameter("linear_curv_change").as_double(); } diff --git a/opennav_coverage/include/opennav_coverage/route_generator.hpp b/opennav_coverage/include/opennav_coverage/route_generator.hpp index 9857ca14..361bbf9a 100644 --- a/opennav_coverage/include/opennav_coverage/route_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/route_generator.hpp @@ -21,8 +21,8 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "opennav_coverage_msgs/msg/route_mode.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_coverage/types.hpp" @@ -43,17 +43,17 @@ class RouteGenerator template explicit RouteGenerator(const NodeT & node) { - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_route_type", rclcpp::ParameterValue("BOUSTROPHEDON")); std::string type_str = node->get_parameter("default_route_type").as_string(); default_type_ = toType(type_str); default_generator_ = createGenerator(default_type_); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_spiral_n", rclcpp::ParameterValue(4)); default_spiral_n_ = node->get_parameter("default_spiral_n").as_int(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_custom_order", rclcpp::PARAMETER_INTEGER_ARRAY); try { // Get the custom order and cast to size_t diff --git a/opennav_coverage/include/opennav_coverage/swath_generator.hpp b/opennav_coverage/include/opennav_coverage/swath_generator.hpp index 8df76420..e34302ec 100644 --- a/opennav_coverage/include/opennav_coverage/swath_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/swath_generator.hpp @@ -22,8 +22,8 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "opennav_coverage_msgs/msg/swath_mode.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_coverage/types.hpp" @@ -48,25 +48,25 @@ class SwathGenerator logger_ = node->get_logger(); robot_params_ = robot_params; - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_swath_type", rclcpp::ParameterValue("LENGTH")); std::string type_str = node->get_parameter("default_swath_type").as_string(); default_type_ = toType(type_str); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_swath_angle_type", rclcpp::ParameterValue("BRUTE_FORCE")); std::string angle_str = node->get_parameter("default_swath_angle_type").as_string(); default_angle_type_ = toAngleType(angle_str); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_swath_angle", rclcpp::ParameterValue(0.0)); default_swath_angle_ = node->get_parameter("default_swath_angle").as_double(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_step_angle", rclcpp::ParameterValue(1.7453e-2)); default_step_angle_ = node->get_parameter("default_step_angle").as_double(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_allow_overlap", rclcpp::ParameterValue(false)); default_allow_overlap_ = node->get_parameter("default_allow_overlap").as_bool(); diff --git a/opennav_coverage/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp index 43ead777..5cb88af4 100644 --- a/opennav_coverage/include/opennav_coverage/utils.hpp +++ b/opennav_coverage/include/opennav_coverage/utils.hpp @@ -24,7 +24,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" #include "opennav_coverage/types.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/point32.hpp" diff --git a/opennav_coverage/package.xml b/opennav_coverage/package.xml index 628e4206..48ed96a0 100644 --- a/opennav_coverage/package.xml +++ b/opennav_coverage/package.xml @@ -15,6 +15,7 @@ rclcpp_lifecycle nav2_util nav2_msgs + nav2_ros_common nav_msgs geometry_msgs builtin_interfaces diff --git a/opennav_coverage/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp index 4e19c651..016eed52 100644 --- a/opennav_coverage/src/coverage_server.cpp +++ b/opennav_coverage/src/coverage_server.cpp @@ -22,12 +22,12 @@ namespace opennav_coverage { CoverageServer::CoverageServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("coverage_server", "", options) +: nav2::LifecycleNode("coverage_server", "", options) { RCLCPP_INFO(get_logger(), "Creating %s", get_name()); } -nav2_util::CallbackReturn +nav2::CallbackReturn CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); @@ -42,30 +42,22 @@ CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) // If in GPS coordinates, we must convert to a CRS to compute coverage // Then, reconvert back to GPS for the user. - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "coordinates_in_cartesian_frame", rclcpp::ParameterValue(true)); get_parameter("coordinates_in_cartesian_frame", cartesian_frame_); - double action_server_result_timeout = 10.0; - nav2_util::declare_parameter_if_not_declared( - node, "action_server_result_timeout", rclcpp::ParameterValue(10.0)); - get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - // Create the action servers for path planning to a pose and through poses - action_server_ = std::make_unique( - shared_from_this(), + action_server_ = node->create_action_server( "compute_coverage_path", std::bind(&CoverageServer::computeCoveragePath, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn CoverageServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating %s", get_name()); @@ -80,10 +72,10 @@ CoverageServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // create bond connection createBond(); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn CoverageServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating %s", get_name()); @@ -94,10 +86,10 @@ CoverageServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // destroy bond connection destroyBond(); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn CoverageServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up %s", get_name()); @@ -108,14 +100,14 @@ CoverageServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) swath_gen_.reset(); headland_gen_.reset(); robot_params_.reset(); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn CoverageServer::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Shutting down %s", get_name()); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } diff --git a/opennav_coverage/test/test_headland.cpp b/opennav_coverage/test/test_headland.cpp index 84b928c0..eed658a4 100644 --- a/opennav_coverage/test/test_headland.cpp +++ b/opennav_coverage/test/test_headland.cpp @@ -15,7 +15,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage/headland_generator.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "fields2cover/utils/random.h" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage/test/test_path.cpp b/opennav_coverage/test/test_path.cpp index e0b7563a..049698f3 100644 --- a/opennav_coverage/test/test_path.cpp +++ b/opennav_coverage/test/test_path.cpp @@ -18,7 +18,7 @@ #include "opennav_coverage/route_generator.hpp" #include "opennav_coverage/swath_generator.hpp" #include "opennav_coverage/path_generator.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "fields2cover/utils/random.h" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage/test/test_robot.cpp b/opennav_coverage/test/test_robot.cpp index 227f6001..f33e4a49 100644 --- a/opennav_coverage/test/test_robot.cpp +++ b/opennav_coverage/test/test_robot.cpp @@ -15,7 +15,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage/robot_params.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage/test/test_route.cpp b/opennav_coverage/test/test_route.cpp index ac11d3ea..5c8d8037 100644 --- a/opennav_coverage/test/test_route.cpp +++ b/opennav_coverage/test/test_route.cpp @@ -17,7 +17,7 @@ #include "opennav_coverage/robot_params.hpp" #include "opennav_coverage/route_generator.hpp" #include "opennav_coverage/swath_generator.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "fields2cover/utils/random.h" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage/test/test_server.cpp b/opennav_coverage/test/test_server.cpp index edd033be..859dbf93 100644 --- a/opennav_coverage/test/test_server.cpp +++ b/opennav_coverage/test/test_server.cpp @@ -16,7 +16,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "opennav_coverage/coverage_server.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" // Luckily, F2C has very high test coverage so we only need to test what we touch @@ -86,7 +86,7 @@ TEST(ServerTest, testServerTransactions) rclcpp_lifecycle::State state; node->configure(state); node->activate(state); - auto node_thread = std::make_unique(node); + auto node_thread = std::make_unique(node); // Send some requests auto client_node = std::make_shared("my_node"); diff --git a/opennav_coverage/test/test_swath.cpp b/opennav_coverage/test/test_swath.cpp index 997cec13..173ac332 100644 --- a/opennav_coverage/test/test_swath.cpp +++ b/opennav_coverage/test/test_swath.cpp @@ -16,7 +16,7 @@ #include "rclcpp/rclcpp.hpp" #include "opennav_coverage/robot_params.hpp" #include "opennav_coverage/swath_generator.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "fields2cover/utils/random.h" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage/test/test_utils.cpp b/opennav_coverage/test/test_utils.cpp index fd8b63fc..26a158f8 100644 --- a/opennav_coverage/test/test_utils.cpp +++ b/opennav_coverage/test/test_utils.cpp @@ -17,7 +17,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage/utils.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage/test/test_viz.cpp b/opennav_coverage/test/test_viz.cpp index 1c5f63cf..df06e849 100644 --- a/opennav_coverage/test/test_viz.cpp +++ b/opennav_coverage/test/test_viz.cpp @@ -15,7 +15,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage/visualizer.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" // Luckily, F2C has very high test coverage so we only need to test what we touch diff --git a/opennav_coverage_bt/CMakeLists.txt b/opennav_coverage_bt/CMakeLists.txt index 3d3aaded..f1e21ace 100644 --- a/opennav_coverage_bt/CMakeLists.txt +++ b/opennav_coverage_bt/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_bt_navigator REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_behavior_tree REQUIRED) @@ -27,6 +28,7 @@ set(dependencies rclcpp_action std_msgs nav2_util + nav2_bt_navigator nav2_msgs nav2_core nav2_behavior_tree diff --git a/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml b/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml index f8c9d1b4..c7e4b357 100644 --- a/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml +++ b/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml @@ -19,7 +19,7 @@ 'polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}"' if set polygon via NavigateCompleteCoverage or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage --> - + diff --git a/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml b/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml index c4e7255c..7990678b 100644 --- a/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml +++ b/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml @@ -24,11 +24,11 @@ - - + + - + diff --git a/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml b/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml index 7743d92e..7fe30afc 100644 --- a/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml +++ b/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml @@ -19,7 +19,7 @@ 'polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}"' if set polygon via NavigateCompleteCoverage or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage --> - + diff --git a/opennav_coverage_bt/package.xml b/opennav_coverage_bt/package.xml index f207c3b1..40ab7880 100644 --- a/opennav_coverage_bt/package.xml +++ b/opennav_coverage_bt/package.xml @@ -12,6 +12,7 @@ rclcpp rclcpp_action nav2_behavior_tree + nav2_bt_navigator nav2_util nav2_core nav2_msgs diff --git a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp index aa748931..9a6491b8 100644 --- a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp +++ b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp @@ -49,7 +49,7 @@ class CancelCoverageActionTestFixture : public ::testing::Test public: static void SetUpTestCase() { - node_ = std::make_shared("cancel_compute_coverage_path_test_fixture"); + node_ = std::make_shared("cancel_compute_coverage_path_test_fixture"); factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); @@ -57,7 +57,7 @@ class CancelCoverageActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -104,13 +104,13 @@ class CancelCoverageActionTestFixture : public ::testing::Test rclcpp_action::Client> client_; protected: - static rclcpp::Node::SharedPtr node_; + static nav2::LifecycleNode::SharedPtr node_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; -rclcpp::Node::SharedPtr CancelCoverageActionTestFixture::node_ = nullptr; +nav2::LifecycleNode::SharedPtr CancelCoverageActionTestFixture::node_ = nullptr; std::shared_ptr CancelCoverageActionTestFixture::action_server_ = nullptr; std::shared_ptr> diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index afa81d82..b5973064 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -55,7 +55,7 @@ class ComputeCoveragePathActionTestFixture : public ::testing::Test public: static void SetUpTestCase() { - node_ = std::make_shared("compute_coverage_path_action_test_fixture"); + node_ = std::make_shared("compute_coverage_path_action_test_fixture"); factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); @@ -63,7 +63,7 @@ class ComputeCoveragePathActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -105,13 +105,13 @@ class ComputeCoveragePathActionTestFixture : public ::testing::Test static std::shared_ptr action_server_; protected: - static rclcpp::Node::SharedPtr node_; + static nav2::LifecycleNode::SharedPtr node_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; static std::shared_ptr tree_; }; -rclcpp::Node::SharedPtr ComputeCoveragePathActionTestFixture::node_ = nullptr; +nav2::LifecycleNode::SharedPtr ComputeCoveragePathActionTestFixture::node_ = nullptr; std::shared_ptr ComputeCoveragePathActionTestFixture::action_server_ = nullptr; BT::NodeConfiguration * ComputeCoveragePathActionTestFixture::config_ = nullptr; diff --git a/opennav_coverage_msgs/action/NavigateCompleteCoverage.action b/opennav_coverage_msgs/action/NavigateCompleteCoverage.action index 5ac12da6..0f5d8d13 100644 --- a/opennav_coverage_msgs/action/NavigateCompleteCoverage.action +++ b/opennav_coverage_msgs/action/NavigateCompleteCoverage.action @@ -14,8 +14,13 @@ string behavior_tree # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 +uint16 UNKNOWN=800 +uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=801 +uint16 TF_ERROR=802 uint16 error_code +string error_msg + --- #feedback definition diff --git a/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp b/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp index cceb8842..5287d97a 100644 --- a/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp +++ b/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp @@ -19,15 +19,16 @@ #include #include +#include "opennav_coverage_msgs/action/navigate_complete_coverage.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" -#include "opennav_coverage_msgs/action/navigate_complete_coverage.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_util/odometry_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" #include "nav_msgs/msg/path.hpp" -#include "nav2_util/odometry_utils.hpp" namespace opennav_coverage_navigator { @@ -55,7 +56,7 @@ class CoverageNavigator * @param odom_smoother Object to get current smoothed robot's speed */ bool configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr node, + nav2::LifecycleNode::WeakPtr node, std::shared_ptr odom_smoother) override; /** @@ -74,7 +75,7 @@ class CoverageNavigator * @param node WeakPtr to the lifecycle node * @return string Filepath to default XML */ - std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + std::string getDefaultBTFilepath(nav2::LifecycleNode::WeakPtr node) override; protected: /** @@ -111,8 +112,9 @@ class CoverageNavigator /** * @brief Goal pose initialization on the blackboard * @param goal Action template's goal message to process + * @return bool if goal was initialized successfully to be processed */ - void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); + bool initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); rclcpp::Time start_time_; std::string path_blackboard_id_, field_blackboard_id_, polygon_blackboard_id_; diff --git a/opennav_coverage_navigator/package.xml b/opennav_coverage_navigator/package.xml index f66c1fcb..d9c77d33 100644 --- a/opennav_coverage_navigator/package.xml +++ b/opennav_coverage_navigator/package.xml @@ -15,6 +15,7 @@ nav2_util nav2_core nav2_msgs + nav2_ros_common nav_msgs geometry_msgs opennav_coverage_msgs @@ -28,6 +29,6 @@ ament_cmake - + diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 952dd933..a72e4b8f 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -23,54 +23,48 @@ namespace opennav_coverage_navigator bool CoverageNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + nav2::LifecycleNode::WeakPtr parent_node, std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - if (!node->has_parameter("path_blackboard_id")) { - node->declare_parameter("path_blackboard_id", std::string("path")); - } - path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); + path_blackboard_id_ = node->declare_or_get_parameter( + "path_blackboard_id", std::string("path")); + field_blackboard_id_ = node->declare_or_get_parameter( + "field_file_blackboard_id", std::string("field_filepath")); + polygon_blackboard_id_ = node->declare_or_get_parameter( + "field_polygon_blackboard_id", std::string("field_polygon")); + polygon_frame_blackboard_id_ = node->declare_or_get_parameter( + "polygon_frame_blackboard_id", std::string("polygon_frame_id")); - if (!node->has_parameter("field_file_blackboard_id")) { - node->declare_parameter("field_file_blackboard_id", std::string("field_filepath")); - } - field_blackboard_id_ = node->get_parameter("field_file_blackboard_id").as_string(); + // Odometry smoother object for getting current speed + odom_smoother_ = odom_smoother; - if (!node->has_parameter("field_polygon_blackboard_id")) { - node->declare_parameter("field_polygon_blackboard_id", std::string("field_polygon")); - } - polygon_blackboard_id_ = node->get_parameter("field_polygon_blackboard_id").as_string(); + // Groot monitoring + const bool enable_groot_monitoring = node->declare_or_get_parameter( + getName() + ".enable_groot_monitoring", false); + const int groot_server_port = node->declare_or_get_parameter( + getName() + ".groot_server_port", 1667); - if (!node->has_parameter("polygon_frame_blackboard_id")) { - node->declare_parameter("polygon_frame_blackboard_id", std::string("polygon_frame_id")); - } - polygon_frame_blackboard_id_ = node->get_parameter("polygon_frame_blackboard_id").as_string(); + bt_action_server_->setGrootMonitoring(enable_groot_monitoring, groot_server_port); - // Odometry smoother object for getting current speed - odom_smoother_ = odom_smoother; return true; } std::string CoverageNavigator::getDefaultBTFilepath( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + nav2::LifecycleNode::WeakPtr parent_node) { - std::string default_bt_xml_filename; auto node = parent_node.lock(); - if (!node->has_parameter("default_coverage_bt_xml")) { - std::string pkg_share_dir = - ament_index_cpp::get_package_share_directory("opennav_coverage_bt"); - node->declare_parameter( - "default_coverage_bt_xml", - pkg_share_dir + - "/behavior_trees/navigate_w_basic_complete_coverage.xml"); - } + const std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("opennav_coverage_bt"); - node->get_parameter("default_coverage_bt_xml", default_bt_xml_filename); + const auto default_bt_xml_filename = node->declare_or_get_parameter( + "default_coverage_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_w_basic_complete_coverage.xml"); return default_bt_xml_filename; } @@ -90,11 +84,12 @@ CoverageNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) RCLCPP_ERROR( logger_, "BT file not found: %s. Navigation canceled.", bt_xml_filename.c_str()); + bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, + std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled."); return false; } - initializeGoalPose(goal); - return true; + return initializeGoalPose(goal); } void @@ -185,7 +180,13 @@ CoverageNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - initializeGoalPose(bt_action_server_->acceptPendingGoal()); + if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the goal could not be " + "initialized. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } } else { RCLCPP_WARN( logger_, @@ -198,14 +199,19 @@ CoverageNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) } } -void +bool CoverageNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + bt_action_server_->setInternalError(ActionT::Result::TF_ERROR, + "Initial robot pose is not available."); + return false; + } if (goal->field_filepath.size() == 0) { RCLCPP_INFO( @@ -227,6 +233,8 @@ CoverageNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) blackboard->set>( polygon_blackboard_id_, goal->polygons); blackboard->set(polygon_frame_blackboard_id_, goal->frame_id); + + return true; } } // namespace opennav_coverage_navigator diff --git a/opennav_coverage_navigator/test/test_coverage_navigator.cpp b/opennav_coverage_navigator/test/test_coverage_navigator.cpp index e5bdc2d9..7cab8c87 100644 --- a/opennav_coverage_navigator/test/test_coverage_navigator.cpp +++ b/opennav_coverage_navigator/test/test_coverage_navigator.cpp @@ -15,10 +15,11 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage_navigator/coverage_navigator.hpp" -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" class RosLockGuard { @@ -66,7 +67,7 @@ inline std::vector getLibs() "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", + "nav2_path_expiring_timer_condition_bt_node", "nav2_distance_traveled_condition_bt_node", "nav2_single_trigger_bt_node", "nav2_goal_updated_controller_bt_node", @@ -93,7 +94,7 @@ inline std::vector getLibs() TEST(CoverageNavigatorTests, TestBasicFunctionality) { opennav_coverage_navigator::CoverageNavigator navigator; - auto node = std::make_shared("test_node"); + auto node = std::make_shared("test_node"); auto odom_smoother = std::make_shared(node, 0.3, "odom"); nav2_core::NavigatorMuxer plugin_muxer; @@ -129,7 +130,7 @@ TEST(CoverageNavigatorTests, TestBasicServer) { // Create server opennav_coverage_navigator::CoverageNavigator navigator; - auto node = std::make_shared("test_node"); + auto node = std::make_shared("test_node"); auto odom_smoother = std::make_shared(node, 0.3, "odom"); nav2_core::NavigatorMuxer plugin_muxer; @@ -147,7 +148,7 @@ TEST(CoverageNavigatorTests, TestBasicServer) navigator.on_configure(node, getLibs(), feedback_utils, &plugin_muxer, odom_smoother); navigator.on_activate(); - auto node_thread = std::make_unique(node); + auto node_thread = std::make_unique(node); // Call server auto client_node = std::make_shared("my_node"); diff --git a/opennav_row_coverage/include/opennav_row_coverage/row_coverage_server.hpp b/opennav_row_coverage/include/opennav_row_coverage/row_coverage_server.hpp index d75ff5b5..f8aef5d8 100644 --- a/opennav_row_coverage/include/opennav_row_coverage/row_coverage_server.hpp +++ b/opennav_row_coverage/include/opennav_row_coverage/row_coverage_server.hpp @@ -21,9 +21,9 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_util/simple_action_server.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/simple_action_server.hpp" #include "opennav_row_coverage/row_swath_generator.hpp" #include "opennav_coverage/route_generator.hpp" #include "opennav_coverage/path_generator.hpp" @@ -36,10 +36,10 @@ namespace opennav_row_coverage * @brief An action server which implements highly reconfigurable complete * coverage planning using the Fields2Cover library with pre-established rows */ -class RowCoverageServer : public nav2_util::LifecycleNode +class RowCoverageServer : public nav2::LifecycleNode { public: - using ActionServer = nav2_util::SimpleActionServer; + using ActionServer = nav2::SimpleActionServer; /** * @brief A constructor for opennav_row_coverage::RowCoverageServer @@ -78,35 +78,35 @@ class RowCoverageServer : public nav2_util::LifecycleNode * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; /** * @brief Activate member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; /** * @brief Deactivate member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; /** * @brief Reset member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; /** * @brief Called when in shutdown state * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; /** * @brief Callback executed when a parameter change is detected @@ -119,7 +119,7 @@ class RowCoverageServer : public nav2_util::LifecycleNode rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; - std::unique_ptr action_server_; + typename ActionServer::SharedPtr action_server_; std::unique_ptr robot_params_; std::unique_ptr swath_gen_; diff --git a/opennav_row_coverage/include/opennav_row_coverage/row_swath_generator.hpp b/opennav_row_coverage/include/opennav_row_coverage/row_swath_generator.hpp index 0b241c8b..95fc2be2 100644 --- a/opennav_row_coverage/include/opennav_row_coverage/row_swath_generator.hpp +++ b/opennav_row_coverage/include/opennav_row_coverage/row_swath_generator.hpp @@ -22,7 +22,7 @@ #include "opennav_row_coverage/types.hpp" #include "rclcpp/rclcpp.hpp" -#include "nav2_util/node_utils.hpp" +#include "nav2_ros_common/node_utils.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_row_coverage/utils.hpp" @@ -96,12 +96,12 @@ class RowSwathGenerator explicit RowSwathGenerator(const NodeT & node) { logger_ = node->get_logger(); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_swath_type", rclcpp::ParameterValue("CENTER")); std::string type_str = node->get_parameter("default_swath_type").as_string(); default_type_ = toType(type_str); - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "default_offset", rclcpp::ParameterValue(0.5)); default_offset_ = static_cast(node->get_parameter("default_offset").as_double()); } diff --git a/opennav_row_coverage/package.xml b/opennav_row_coverage/package.xml index 9538a821..9d87f2f6 100644 --- a/opennav_row_coverage/package.xml +++ b/opennav_row_coverage/package.xml @@ -11,6 +11,7 @@ rclcpp nav2_util + nav2_ros_common fields2cover opennav_coverage opennav_coverage_msgs diff --git a/opennav_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp index 16999608..86dbb760 100644 --- a/opennav_row_coverage/src/row_coverage_server.cpp +++ b/opennav_row_coverage/src/row_coverage_server.cpp @@ -22,12 +22,12 @@ namespace opennav_row_coverage { RowCoverageServer::RowCoverageServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("row_coverage_server", "", options) +: nav2::LifecycleNode("row_coverage_server", "", options) { RCLCPP_INFO(get_logger(), "Creating %s", get_name()); } -nav2_util::CallbackReturn +nav2::CallbackReturn RowCoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); @@ -41,35 +41,27 @@ RowCoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) // If in GPS coordinates, we must convert to a CRS to compute coverage // Then, reconvert back to GPS for the user. - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "coordinates_in_cartesian_frame", rclcpp::ParameterValue(true)); get_parameter("coordinates_in_cartesian_frame", cartesian_frame_); // Whether to reorder IDs in file by value instead of file ordering - nav2_util::declare_parameter_if_not_declared( + nav2::declare_parameter_if_not_declared( node, "order_ids", rclcpp::ParameterValue(true)); get_parameter("order_ids", order_ids_); - double action_server_result_timeout = 10.0; - nav2_util::declare_parameter_if_not_declared( - node, "action_server_result_timeout", rclcpp::ParameterValue(10.0)); - get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - // Create the action servers for path planning to a pose and through poses - action_server_ = std::make_unique( - shared_from_this(), + action_server_ = node->create_action_server( "compute_coverage_path", std::bind(&RowCoverageServer::computeCoveragePath, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn RowCoverageServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating %s", get_name()); @@ -84,10 +76,10 @@ RowCoverageServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // create bond connection createBond(); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn RowCoverageServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating %s", get_name()); @@ -98,10 +90,10 @@ RowCoverageServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // destroy bond connection destroyBond(); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn RowCoverageServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up %s", get_name()); @@ -111,14 +103,14 @@ RowCoverageServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) route_gen_.reset(); swath_gen_.reset(); robot_params_.reset(); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn +nav2::CallbackReturn RowCoverageServer::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Shutting down %s", get_name()); - return nav2_util::CallbackReturn::SUCCESS; + return nav2::CallbackReturn::SUCCESS; } diff --git a/opennav_row_coverage/test/test_server.cpp b/opennav_row_coverage/test/test_server.cpp index 086f4058..378879e4 100644 --- a/opennav_row_coverage/test/test_server.cpp +++ b/opennav_row_coverage/test/test_server.cpp @@ -16,7 +16,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "opennav_coverage/coverage_server.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" // Luckily, F2C has very high test coverage so we only need to test what we touch @@ -86,7 +86,7 @@ TEST(ServerTest, testServerTransactions) rclcpp_lifecycle::State state; node->configure(state); node->activate(state); - auto node_thread = std::make_unique(node); + auto node_thread = std::make_unique(node); // Send some requests auto client_node = std::make_shared("my_node"); diff --git a/opennav_row_coverage/test/test_utils.cpp b/opennav_row_coverage/test/test_utils.cpp index 264df40b..ed0e5241 100644 --- a/opennav_row_coverage/test/test_utils.cpp +++ b/opennav_row_coverage/test/test_utils.cpp @@ -17,7 +17,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_row_coverage/utils.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" class RosLockGuard