From 0922cf10c7ab665e6ea0eb2d9fda707a66e72c18 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 27 Jun 2025 09:27:47 +0000 Subject: [PATCH 1/7] update msg for Kilted --- opennav_coverage_msgs/action/NavigateCompleteCoverage.action | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/opennav_coverage_msgs/action/NavigateCompleteCoverage.action b/opennav_coverage_msgs/action/NavigateCompleteCoverage.action index 5ac12da6..1eba3efa 100644 --- a/opennav_coverage_msgs/action/NavigateCompleteCoverage.action +++ b/opennav_coverage_msgs/action/NavigateCompleteCoverage.action @@ -14,8 +14,12 @@ 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 error_code +string error_msg + --- #feedback definition From f1766e127ac2dac79ff6baba5973bd3c07134395 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 14 Jul 2025 14:05:08 +0200 Subject: [PATCH 2/7] [opennav_coverage_bt] migrate BT xml files --- .../behavior_trees/navigate_w_basic_complete_coverage.xml | 4 ++-- .../navigate_w_basic_complete_coverage_nav_to_start.xml | 8 ++++---- .../navigate_w_basic_row_complete_coverage.xml | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) 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..8b6b3f14 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 @@ -18,8 +18,8 @@ - - + + 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..9e02aef5 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 @@ -20,15 +20,15 @@ or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage --> - + - - + + - + 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..e828a349 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 @@ -18,8 +18,8 @@ - - + + From 394410d7598ea5eabd61bff0cce06a5a3f2a5f33 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 14 Jul 2025 14:05:28 +0200 Subject: [PATCH 3/7] migrate coverage_navigator --- .../action/NavigateCompleteCoverage.action | 1 + .../coverage_navigator.hpp | 3 +- .../src/coverage_navigator.cpp | 44 +++++++++++++++---- 3 files changed, 39 insertions(+), 9 deletions(-) diff --git a/opennav_coverage_msgs/action/NavigateCompleteCoverage.action b/opennav_coverage_msgs/action/NavigateCompleteCoverage.action index 1eba3efa..0f5d8d13 100644 --- a/opennav_coverage_msgs/action/NavigateCompleteCoverage.action +++ b/opennav_coverage_msgs/action/NavigateCompleteCoverage.action @@ -16,6 +16,7 @@ string behavior_tree uint16 NONE=0 uint16 UNKNOWN=800 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=801 +uint16 TF_ERROR=802 uint16 error_code string error_msg 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 56ba125fd26d72cf27465a0d3f2f53871d97409c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 16 Jul 2025 11:37:56 +0200 Subject: [PATCH 4/7] rm error_msg from ComputeCoveragePath --- .../behavior_trees/navigate_w_basic_complete_coverage.xml | 2 +- .../navigate_w_basic_complete_coverage_nav_to_start.xml | 2 +- .../behavior_trees/navigate_w_basic_row_complete_coverage.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 8b6b3f14..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 @@ -18,7 +18,7 @@ - + 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 9e02aef5..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 @@ -20,7 +20,7 @@ or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage --> - + 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 e828a349..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 @@ -18,7 +18,7 @@ - + From e80677481cab7ef6f683ee46e761fd04bb1d0b8a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 4 Aug 2025 09:22:32 +0000 Subject: [PATCH 5/7] use newer rostooling docker images, run test for Kilted --- .github/workflows/lint.yml | 2 +- .github/workflows/test.yml | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index b20206a2..78d07e93 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-desktop:master strategy: fail-fast: false matrix: diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e52fdb96..cad1988a 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-desktop: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 }} From eee99ec793e9f8ac006dc412f2caeed3bd6c906a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 4 Aug 2025 11:50:25 +0200 Subject: [PATCH 6/7] run linter on Kilted --- .github/workflows/lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 78d07e93..5c8d0cb5 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -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 From ebefc3d8907213bd7bfb9101bb41a47a057d4d43 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 7 Aug 2025 06:43:42 +0000 Subject: [PATCH 7/7] use base image --- .github/workflows/lint.yml | 2 +- .github/workflows/test.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 5c8d0cb5..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: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-desktop:master + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-ros-base:master strategy: fail-fast: false matrix: diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index cad1988a..d10062b0 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -12,7 +12,7 @@ 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-desktop:master + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-ros-base:master strategy: fail-fast: false matrix: