From d6e41a298c5e9767cc2a42c9fafa7ca41b9bf089 Mon Sep 17 00:00:00 2001 From: Adi Vardi <57910756+adivardi@users.noreply.github.com> Date: Thu, 7 Aug 2025 16:45:39 +0200 Subject: [PATCH 01/17] Migrate to Kilted (#94) * update msg for Kilted * [opennav_coverage_bt] migrate BT xml files * migrate coverage_navigator * rm error_msg from ComputeCoveragePath * use newer rostooling docker images, run test for Kilted * run linter on Kilted * use base image --- .github/workflows/lint.yml | 4 +- .github/workflows/test.yml | 6 +-- .../navigate_w_basic_complete_coverage.xml | 2 +- ...w_basic_complete_coverage_nav_to_start.xml | 6 +-- ...navigate_w_basic_row_complete_coverage.xml | 2 +- .../action/NavigateCompleteCoverage.action | 5 +++ .../coverage_navigator.hpp | 3 +- .../src/coverage_navigator.cpp | 44 +++++++++++++++---- 8 files changed, 53 insertions(+), 19 deletions(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index b20206a2..ca9b9418 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-kilted-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: kilted package-name: | opennav_coverage opennav_coverage_bt diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e52fdb96..d10062b0 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -12,18 +12,18 @@ 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-kilted-ros-base:master strategy: fail-fast: false matrix: - ros_distro: [jazzy] + ros_distro: [kilted] 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: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: ${{ matrix.ros_distro }} 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_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..3fb0dfae 100644 --- a/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp +++ b/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp @@ -111,8 +111,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/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 952dd933..93e9e5f2 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -51,6 +51,20 @@ CoverageNavigator::configure( // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; + + // Groot monitoring + if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { + node->declare_parameter(getName() + ".enable_groot_monitoring", false); + } + + if (!node->has_parameter(getName() + ".groot_server_port")) { + node->declare_parameter(getName() + ".groot_server_port", 1667); + } + + bt_action_server_->setGrootMonitoring( + node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(), + node->get_parameter(getName() + ".groot_server_port").as_int()); + return true; } @@ -90,11 +104,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 +200,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 +219,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 +253,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 From c65372307f5c73ab0836b4e7cc1196274cdd987e Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 1 Aug 2025 11:14:43 +0200 Subject: [PATCH 02/17] Migrate to Lyrical --- README.md | 9 ++-- .../opennav_coverage/coverage_server.hpp | 22 ++++---- .../opennav_coverage/headland_generator.hpp | 2 +- .../opennav_coverage/path_generator.hpp | 2 +- .../include/opennav_coverage/robot_params.hpp | 2 +- .../opennav_coverage/route_generator.hpp | 2 +- .../opennav_coverage/swath_generator.hpp | 2 +- opennav_coverage/package.xml | 1 + opennav_coverage/src/coverage_server.cpp | 34 +++++------- .../coverage_navigator.hpp | 9 ++-- opennav_coverage_navigator/package.xml | 3 +- .../src/coverage_navigator.cpp | 54 +++++-------------- .../test/test_coverage_navigator.cpp | 5 +- .../row_coverage_server.hpp | 22 ++++---- opennav_row_coverage/package.xml | 1 + .../src/row_coverage_server.cpp | 34 +++++------- 16 files changed, 83 insertions(+), 121 deletions(-) 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..5b331106 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.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..576a77f5 100644 --- a/opennav_coverage/include/opennav_coverage/headland_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/headland_generator.hpp @@ -21,7 +21,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" +#include "nav2_ros_common/lifecycle_node.hpp.hpp" #include "nav2_util/node_utils.hpp" #include "opennav_coverage_msgs/msg/headland_mode.hpp" #include "opennav_coverage/utils.hpp" diff --git a/opennav_coverage/include/opennav_coverage/path_generator.hpp b/opennav_coverage/include/opennav_coverage/path_generator.hpp index aa70984e..42985acd 100644 --- a/opennav_coverage/include/opennav_coverage/path_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/path_generator.hpp @@ -22,7 +22,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" +#include "nav2_ros_common/lifecycle_node.hpp.hpp" #include "nav2_util/node_utils.hpp" #include "opennav_coverage_msgs/msg/path_mode.hpp" #include "opennav_coverage/utils.hpp" diff --git a/opennav_coverage/include/opennav_coverage/robot_params.hpp b/opennav_coverage/include/opennav_coverage/robot_params.hpp index 35ba0a2a..05e2add4 100644 --- a/opennav_coverage/include/opennav_coverage/robot_params.hpp +++ b/opennav_coverage/include/opennav_coverage/robot_params.hpp @@ -21,7 +21,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" +#include "nav2_ros_common/lifecycle_node.hpp.hpp" #include "nav2_util/node_utils.hpp" #include "opennav_coverage/utils.hpp" #include "opennav_coverage/types.hpp" diff --git a/opennav_coverage/include/opennav_coverage/route_generator.hpp b/opennav_coverage/include/opennav_coverage/route_generator.hpp index 9857ca14..e2d1ae03 100644 --- a/opennav_coverage/include/opennav_coverage/route_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/route_generator.hpp @@ -21,7 +21,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" +#include "nav2_ros_common/lifecycle_node.hpp.hpp" #include "nav2_util/node_utils.hpp" #include "opennav_coverage_msgs/msg/route_mode.hpp" #include "opennav_coverage/utils.hpp" diff --git a/opennav_coverage/include/opennav_coverage/swath_generator.hpp b/opennav_coverage/include/opennav_coverage/swath_generator.hpp index 8df76420..736f6b7c 100644 --- a/opennav_coverage/include/opennav_coverage/swath_generator.hpp +++ b/opennav_coverage/include/opennav_coverage/swath_generator.hpp @@ -22,7 +22,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_util/lifecycle_node.hpp" +#include "nav2_ros_common/lifecycle_node.hpp.hpp" #include "nav2_util/node_utils.hpp" #include "opennav_coverage_msgs/msg/swath_mode.hpp" #include "opennav_coverage/utils.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..7076ca16 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()); @@ -46,26 +46,18 @@ CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp b/opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp index 3fb0dfae..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: /** 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 93e9e5f2..1666c68b 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -23,68 +23,42 @@ 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(); - - 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(); - - 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(); - - 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(); + node->declare_or_get_parameter("path_blackboard_id", std::string("path")); + node->declare_or_get_parameter("field_file_blackboard_id", std::string("field_filepath")); + node->declare_or_get_parameter("field_polygon_blackboard_id", std::string("field_polygon")); + node->declare_or_get_parameter("polygon_frame_blackboard_id", std::string("polygon_frame_id")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; // Groot monitoring - if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { - node->declare_parameter(getName() + ".enable_groot_monitoring", false); - } + bool enable_groot_monitoring = node->declare_or_get_parameter(getName() + ".enable_groot_monitoring", false); + int groot_server_port = node->declare_or_get_parameter(getName() + ".groot_server_port", 1667); - if (!node->has_parameter(getName() + ".groot_server_port")) { - node->declare_parameter(getName() + ".groot_server_port", 1667); - } - - bt_action_server_->setGrootMonitoring( - node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(), - node->get_parameter(getName() + ".groot_server_port").as_int()); + bt_action_server_->setGrootMonitoring(enable_groot_monitoring, groot_server_port); 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"); - } + 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); + 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; } diff --git a/opennav_coverage_navigator/test/test_coverage_navigator.cpp b/opennav_coverage_navigator/test/test_coverage_navigator.cpp index e5bdc2d9..dc2562cf 100644 --- a/opennav_coverage_navigator/test/test_coverage_navigator.cpp +++ b/opennav_coverage_navigator/test/test_coverage_navigator.cpp @@ -15,6 +15,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage_navigator/coverage_navigator.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/create_timer_ros.h" @@ -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; 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..f7b24d66 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.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/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..be6afe68 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()); @@ -50,26 +50,18 @@ RowCoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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; } From a70d42f31fceb58e676719a04868dabd3f4df257 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 1 Aug 2025 15:48:10 +0200 Subject: [PATCH 03/17] fix all compile errors due to wrong includes and to methods that moved from nav2_util to nav2 namespace --- .../include/opennav_coverage/coverage_server.hpp | 2 +- .../opennav_coverage/headland_generator.hpp | 8 ++++---- .../include/opennav_coverage/path_generator.hpp | 10 +++++----- .../include/opennav_coverage/robot_params.hpp | 12 ++++++------ .../include/opennav_coverage/route_generator.hpp | 10 +++++----- .../include/opennav_coverage/swath_generator.hpp | 14 +++++++------- .../include/opennav_coverage/utils.hpp | 2 +- opennav_coverage/src/coverage_server.cpp | 2 +- opennav_coverage/test/test_server.cpp | 2 +- .../test/test_coverage_navigator.cpp | 2 +- .../opennav_row_coverage/row_coverage_server.hpp | 2 +- .../opennav_row_coverage/row_swath_generator.hpp | 6 +++--- opennav_row_coverage/src/row_coverage_server.cpp | 4 ++-- opennav_row_coverage/test/test_server.cpp | 2 +- 14 files changed, 39 insertions(+), 39 deletions(-) diff --git a/opennav_coverage/include/opennav_coverage/coverage_server.hpp b/opennav_coverage/include/opennav_coverage/coverage_server.hpp index 5b331106..54225189 100644 --- a/opennav_coverage/include/opennav_coverage/coverage_server.hpp +++ b/opennav_coverage/include/opennav_coverage/coverage_server.hpp @@ -21,7 +21,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_ros_common/lifecycle_node.hpp.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" diff --git a/opennav_coverage/include/opennav_coverage/headland_generator.hpp b/opennav_coverage/include/opennav_coverage/headland_generator.hpp index 576a77f5..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_ros_common/lifecycle_node.hpp.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 42985acd..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_ros_common/lifecycle_node.hpp.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 05e2add4..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_ros_common/lifecycle_node.hpp.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 e2d1ae03..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_ros_common/lifecycle_node.hpp.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 736f6b7c..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_ros_common/lifecycle_node.hpp.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/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp index 7076ca16..016eed52 100644 --- a/opennav_coverage/src/coverage_server.cpp +++ b/opennav_coverage/src/coverage_server.cpp @@ -42,7 +42,7 @@ 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_); diff --git a/opennav_coverage/test/test_server.cpp b/opennav_coverage/test/test_server.cpp index edd033be..94d22e57 100644 --- a/opennav_coverage/test/test_server.cpp +++ b/opennav_coverage/test/test_server.cpp @@ -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_navigator/test/test_coverage_navigator.cpp b/opennav_coverage_navigator/test/test_coverage_navigator.cpp index dc2562cf..19f867a7 100644 --- a/opennav_coverage_navigator/test/test_coverage_navigator.cpp +++ b/opennav_coverage_navigator/test/test_coverage_navigator.cpp @@ -148,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 f7b24d66..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,7 +21,7 @@ #include "fields2cover.h" // NOLINT #include "rclcpp/rclcpp.hpp" -#include "nav2_ros_common/lifecycle_node.hpp.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" 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/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp index be6afe68..86dbb760 100644 --- a/opennav_row_coverage/src/row_coverage_server.cpp +++ b/opennav_row_coverage/src/row_coverage_server.cpp @@ -41,12 +41,12 @@ 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_); diff --git a/opennav_row_coverage/test/test_server.cpp b/opennav_row_coverage/test/test_server.cpp index 086f4058..bfe93cec 100644 --- a/opennav_row_coverage/test/test_server.cpp +++ b/opennav_row_coverage/test/test_server.cpp @@ -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"); From 548d8afbc9914fa825d2c680d7331899a231278c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 1 Aug 2025 15:49:51 +0200 Subject: [PATCH 04/17] remove leftover declaration --- opennav_coverage_navigator/src/coverage_navigator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 1666c68b..a80eb3e0 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -50,12 +50,11 @@ std::string CoverageNavigator::getDefaultBTFilepath( nav2::LifecycleNode::WeakPtr parent_node) { - std::string default_bt_xml_filename; auto node = parent_node.lock(); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("opennav_coverage_bt"); - auto default_bt_xml_filename = node->declare_or_get_parameter( + 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"); From 7684231fdacb9c3fbda39ec2da8eeafd59bdf73e Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 4 Aug 2025 10:45:38 +0200 Subject: [PATCH 05/17] fix missing assignment to class members --- opennav_coverage_navigator/src/coverage_navigator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index a80eb3e0..790c666c 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -29,10 +29,10 @@ CoverageNavigator::configure( start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - node->declare_or_get_parameter("path_blackboard_id", std::string("path")); - node->declare_or_get_parameter("field_file_blackboard_id", std::string("field_filepath")); - node->declare_or_get_parameter("field_polygon_blackboard_id", std::string("field_polygon")); - node->declare_or_get_parameter("polygon_frame_blackboard_id", std::string("polygon_frame_id")); + 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")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; From 040341682ef4ff2963d70952dbd46e9dfc9f1233 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 4 Aug 2025 11:10:26 +0200 Subject: [PATCH 06/17] make Blackboard variable ID prive to the navigator to avoid clashes between navigators --- opennav_coverage_navigator/src/coverage_navigator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 790c666c..03854a40 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -29,10 +29,10 @@ CoverageNavigator::configure( start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - 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")); + path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); + field_blackboard_id_ = node->declare_or_get_parameter(getName() + ".field_file_blackboard_id", std::string("field_filepath")); + polygon_blackboard_id_ = node->declare_or_get_parameter(getName() + ".field_polygon_blackboard_id", std::string("field_polygon")); + polygon_frame_blackboard_id_ = node->declare_or_get_parameter(getName() + ".polygon_frame_blackboard_id", std::string("polygon_frame_id")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; From 60e3c91886faf20cc21aaf70a1923ff21b856e93 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 4 Aug 2025 14:56:35 +0200 Subject: [PATCH 07/17] Break long lines --- .../src/coverage_navigator.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 03854a40..cae62db7 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -29,16 +29,21 @@ CoverageNavigator::configure( start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path")); - field_blackboard_id_ = node->declare_or_get_parameter(getName() + ".field_file_blackboard_id", std::string("field_filepath")); - polygon_blackboard_id_ = node->declare_or_get_parameter(getName() + ".field_polygon_blackboard_id", std::string("field_polygon")); - polygon_frame_blackboard_id_ = node->declare_or_get_parameter(getName() + ".polygon_frame_blackboard_id", std::string("polygon_frame_id")); + path_blackboard_id_ = node->declare_or_get_parameter( + getName() + ".path_blackboard_id", std::string("path")); + field_blackboard_id_ = node->declare_or_get_parameter( + getName() + ".field_file_blackboard_id", std::string("field_filepath")); + polygon_blackboard_id_ = node->declare_or_get_parameter( + getName() + ".field_polygon_blackboard_id", std::string("field_polygon")); + polygon_frame_blackboard_id_ = node->declare_or_get_parameter( + getName() + ".polygon_frame_blackboard_id", std::string("polygon_frame_id")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; // Groot monitoring - bool enable_groot_monitoring = node->declare_or_get_parameter(getName() + ".enable_groot_monitoring", false); + bool enable_groot_monitoring = node->declare_or_get_parameter( + getName() + ".enable_groot_monitoring", false); int groot_server_port = node->declare_or_get_parameter(getName() + ".groot_server_port", 1667); bt_action_server_->setGrootMonitoring(enable_groot_monitoring, groot_server_port); From b14cb2cbc1dd080403ef2b52cb8da3d4b9c6785f Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 4 Aug 2025 16:04:53 +0000 Subject: [PATCH 08/17] only run CI on rolling --- .github/workflows/lint.yml | 4 ++-- .github/workflows/test.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index ca9b9418..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: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-ros-base:master + 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: kilted + distribution: rolling package-name: | opennav_coverage opennav_coverage_bt diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index d10062b0..0e979fc3 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -12,11 +12,11 @@ jobs: ROS_DISTRO: ${{ matrix.ros_distro }} RMW_IMPLEMENTATION: rmw_cyclonedds_cpp container: - image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-ros-base:master + 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: [kilted] + ros_distro: [rolling] steps: - uses: actions/checkout@v2 - name: Install Cyclone DDS From 1e7ab7c776363fe39139c7edd54295476b418a73 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 7 Aug 2025 07:05:14 +0000 Subject: [PATCH 09/17] keep BB ID params outside navigator namespace --- opennav_coverage_navigator/src/coverage_navigator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index cae62db7..337aa550 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -30,13 +30,13 @@ CoverageNavigator::configure( auto node = parent_node.lock(); path_blackboard_id_ = node->declare_or_get_parameter( - getName() + ".path_blackboard_id", std::string("path")); + "path_blackboard_id", std::string("path")); field_blackboard_id_ = node->declare_or_get_parameter( - getName() + ".field_file_blackboard_id", std::string("field_filepath")); + "field_file_blackboard_id", std::string("field_filepath")); polygon_blackboard_id_ = node->declare_or_get_parameter( - getName() + ".field_polygon_blackboard_id", std::string("field_polygon")); + "field_polygon_blackboard_id", std::string("field_polygon")); polygon_frame_blackboard_id_ = node->declare_or_get_parameter( - getName() + ".polygon_frame_blackboard_id", std::string("polygon_frame_id")); + "polygon_frame_blackboard_id", std::string("polygon_frame_id")); // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; From 76f32d8275b6d565728c05e47d9b73af4aa1aff8 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Aug 2025 12:45:30 +0000 Subject: [PATCH 10/17] Add const --- opennav_coverage_navigator/src/coverage_navigator.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 337aa550..a72e4b8f 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -42,9 +42,10 @@ CoverageNavigator::configure( odom_smoother_ = odom_smoother; // Groot monitoring - bool enable_groot_monitoring = node->declare_or_get_parameter( + const bool enable_groot_monitoring = node->declare_or_get_parameter( getName() + ".enable_groot_monitoring", false); - int groot_server_port = node->declare_or_get_parameter(getName() + ".groot_server_port", 1667); + const int groot_server_port = node->declare_or_get_parameter( + getName() + ".groot_server_port", 1667); bt_action_server_->setGrootMonitoring(enable_groot_monitoring, groot_server_port); @@ -57,7 +58,8 @@ CoverageNavigator::getDefaultBTFilepath( { auto node = parent_node.lock(); - std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("opennav_coverage_bt"); + const std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("opennav_coverage_bt"); const auto default_bt_xml_filename = node->declare_or_get_parameter( "default_coverage_bt_xml", From 5743b49a17dc2c2cf3b76885b33fd05beb28e5cd Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Aug 2025 13:30:38 +0000 Subject: [PATCH 11/17] [.github] install rosdep before build --- .github/workflows/test.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 0e979fc3..80f7c4cc 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -21,6 +21,13 @@ jobs: - uses: actions/checkout@v2 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + + - name: Update apt and install dependencies with rosdep + run: | + sudo apt-get update + rosdep update + rosdep install --from-paths . --ignore-src -r -y + - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.4 From 52ef31376e591fed69213d07bd6545d98e9a73a8 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Aug 2025 13:55:41 +0000 Subject: [PATCH 12/17] build nav2 from source --- .github/deps.repos | 5 +++++ .github/workflows/test.yml | 8 +------- 2 files changed, 6 insertions(+), 7 deletions(-) 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/test.yml b/.github/workflows/test.yml index 80f7c4cc..4b6fce2a 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -21,17 +21,11 @@ jobs: - uses: actions/checkout@v2 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - - name: Update apt and install dependencies with rosdep - run: | - sudo apt-get update - rosdep update - rosdep install --from-paths . --ignore-src -r -y - - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.4 with: + package-name: 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" From 0c3e724a5a1e9b034c7355fc4fea99ec6aae4545 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Aug 2025 14:56:37 +0000 Subject: [PATCH 13/17] Update tf2 & tf2_ros headers --- opennav_coverage/test/test_headland.cpp | 2 +- opennav_coverage/test/test_path.cpp | 2 +- opennav_coverage/test/test_robot.cpp | 2 +- opennav_coverage/test/test_route.cpp | 2 +- opennav_coverage/test/test_server.cpp | 2 +- opennav_coverage/test/test_swath.cpp | 2 +- opennav_coverage/test/test_utils.cpp | 2 +- opennav_coverage/test/test_viz.cpp | 2 +- .../test/test_coverage_navigator.cpp | 8 ++++---- opennav_row_coverage/test/test_server.cpp | 2 +- opennav_row_coverage/test/test_utils.cpp | 2 +- 11 files changed, 14 insertions(+), 14 deletions(-) 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 94d22e57..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 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_navigator/test/test_coverage_navigator.cpp b/opennav_coverage_navigator/test/test_coverage_navigator.cpp index 19f867a7..198b290e 100644 --- a/opennav_coverage_navigator/test/test_coverage_navigator.cpp +++ b/opennav_coverage_navigator/test/test_coverage_navigator.cpp @@ -16,10 +16,10 @@ #include "rclcpp/rclcpp.hpp" #include "opennav_coverage_navigator/coverage_navigator.hpp" #include "nav2_ros_common/lifecycle_node.hpp" -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/transform_listener.h" +#include "tf2/utils.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/create_timer_ros.hpp" +#include "tf2_ros/transform_listener.hpp" class RosLockGuard { diff --git a/opennav_row_coverage/test/test_server.cpp b/opennav_row_coverage/test/test_server.cpp index bfe93cec..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 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 From be8503c8681e5d391e43d3f6c758f728998cd8ec Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 22 Aug 2025 07:39:41 +0000 Subject: [PATCH 14/17] Migrate nodes in tests --- .../test/test_cancel_complete_coverage.cpp | 8 ++++---- opennav_coverage_bt/test/test_compute_coverage_path.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) 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; From a8927a787fe1215092c57ae0c46406d8dd6804ff Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 27 Aug 2025 09:28:35 +0000 Subject: [PATCH 15/17] depend on nav2_bt_navigator --- .github/workflows/test.yml | 2 +- opennav_coverage_bt/CMakeLists.txt | 2 ++ opennav_coverage_bt/package.xml | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 4b6fce2a..4067c374 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -25,7 +25,7 @@ jobs: id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: opennav_coverage opennav_coverage_bt opennav_coverage_demo opennav_coverage_msgs opennav_coverage_navigator opennav_row_coverage + package-name: nav2_bt_navigator 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/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/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 From b6e8e21540144b3eb5e37291b836c2cf61fe7e26 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 1 Sep 2025 12:31:41 +0000 Subject: [PATCH 16/17] Also build nav2_behavior_tree --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 4067c374..1a70dc12 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -25,7 +25,7 @@ jobs: id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: nav2_bt_navigator opennav_coverage opennav_coverage_bt opennav_coverage_demo opennav_coverage_msgs opennav_coverage_navigator opennav_row_coverage + 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" From 1aac4be66f24f50c3f039cf85d88a6ad8618fcd7 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 1 Sep 2025 13:24:27 +0000 Subject: [PATCH 17/17] Fix naming for nav2_path_expiring_timer_condition_bt_node see https://github.com/ros-navigation/navigation2/pull/5471 --- opennav_coverage_navigator/test/test_coverage_navigator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage_navigator/test/test_coverage_navigator.cpp b/opennav_coverage_navigator/test/test_coverage_navigator.cpp index 198b290e..7cab8c87 100644 --- a/opennav_coverage_navigator/test/test_coverage_navigator.cpp +++ b/opennav_coverage_navigator/test/test_coverage_navigator.cpp @@ -67,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",