Fixes and Improvements

.gitignore

@@ -0,0 +1 @@
+__pycache__/

opennav_coverage/include/opennav_coverage/visualizer.hpp

@@ -46,16 +46,16 @@ class Visualizer
   {
     nav_plan_pub_ = rclcpp::create_publisher<nav_msgs::msg::Path>(
       node->get_node_topics_interface(),
-      "coverage_server/coverage_plan", rclcpp::QoS(1));
+      "coverage_server/coverage_plan", rclcpp::QoS(1).transient_local());
     headlands_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PolygonStamped>(
       node->get_node_topics_interface(),
-      "coverage_server/field_boundary", rclcpp::QoS(1));
+      "coverage_server/field_boundary", rclcpp::QoS(1).transient_local());
     planning_field_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PolygonStamped>(
       node->get_node_topics_interface(),
-      "coverage_server/planning_field", rclcpp::QoS(1));
+      "coverage_server/planning_field", rclcpp::QoS(1).transient_local());
     swaths_pub_ = rclcpp::create_publisher<visualization_msgs::msg::Marker>(
       node->get_node_topics_interface(),
-      "coverage_server/swaths", rclcpp::QoS(1));
+      "coverage_server/swaths", rclcpp::QoS(1).transient_local());
   }
 
   void deactivate();

opennav_coverage/src/visualizer.cpp

@@ -46,6 +46,7 @@ void Visualizer::visualize(
     for (unsigned int i = 0; i != utm_path->poses.size(); i++) {
       utm_path->poses[i].pose.position.x += ref_pt.getX();
       utm_path->poses[i].pose.position.y += ref_pt.getY();
+      utm_path->poses[i].header.frame_id = GLOBAL_FRAME; // Necessary for Foxglove
     }
     nav_plan_pub_->publish(std::move(utm_path));
   }

opennav_coverage_bt/CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -33,7 +33,7 @@ set(dependencies
   nav_msgs
   geometry_msgs
   opennav_coverage_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
 )
 
 add_library(opennav_compute_complete_coverage_action_bt_node SHARED src/compute_complete_coverage_path.cpp)

opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp

@@ -16,7 +16,7 @@
 #define OPENNAV_COVERAGE_BT__UTILS_HPP_
 
 #include <charconv>
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace BT
 {

opennav_coverage_bt/package.xml

@@ -18,7 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>opennav_coverage_msgs</depend>
-  <depend>behaviortree_cpp_v3</depend>
+  <depend>behaviortree_cpp</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

opennav_coverage_bt/src/cancel_complete_coverage_path.cpp

@@ -31,7 +31,7 @@ CoverageCancel::CoverageCancel(
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =

opennav_coverage_bt/src/compute_complete_coverage_path.cpp

@@ -46,6 +46,7 @@ void ComputeCoveragePathAction::on_tick()
     // Convert from vector of Polygons to coverage sp. message
     std::vector<geometry_msgs::msg::Polygon> polys;
     getInput("polygons", polys);
+    goal_.polygons.clear();
     goal_.polygons.resize(polys.size());
     for (unsigned int i = 0; i != polys.size(); i++) {
       for (unsigned int j = 0; j != polys[i].points.size(); j++) {
@@ -98,7 +99,7 @@ void ComputeCoveragePathAction::halt()
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =

opennav_coverage_bt/test/test_cancel_complete_coverage.cpp

@@ -17,7 +17,7 @@
 #include <set>
 #include <string>
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/cancel_complete_coverage_path.hpp"

opennav_coverage_bt/test/test_compute_coverage_path.cpp

@@ -20,7 +20,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/compute_complete_coverage_path.hpp"

opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py

@@ -150,27 +150,33 @@ def main():
     navigator.navigateCoverage(field)
 
     i = 0
-    while not navigator.isTaskComplete():
-        # Do something with the feedback
-        i = i + 1
-        feedback = navigator.getFeedback()
-        if feedback and i % 5 == 0:
-            print('Estimated time of arrival: ' + '{0:.0f}'.format(
-                  Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
-                  + ' seconds.')
-        time.sleep(1)
-
-    # Do something depending on the return code
-    result = navigator.getResult()
-    if result == TaskResult.SUCCEEDED:
-        print('Goal succeeded!')
-    elif result == TaskResult.CANCELED:
-        print('Goal was canceled!')
-    elif result == TaskResult.FAILED:
-        print('Goal failed!')
-    else:
-        print('Goal has an invalid return status!')
+    try:
+        while not navigator.isTaskComplete():
+            # Do something with the feedback
+            i = i + 1
+            feedback = navigator.getFeedback()
+            if feedback and i % 5 == 0:
+                print('Estimated time of arrival: ' + '{0:.0f}'.format(
+                    Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+                    + ' seconds.')
+            time.sleep(1)
+
+        # Do something depending on the return code
+        result = navigator.getResult()
+        if result == TaskResult.SUCCEEDED:
+            print('Goal succeeded!')
+        elif result == TaskResult.CANCELED:
+            print('Goal was canceled!')
+        elif result == TaskResult.FAILED:
+            print('Goal failed!')
+        else:
+            print('Goal has an invalid return status!')
 
+    except KeyboardInterrupt:
+        print("\nCtrl-C detected. Cancelling goal")
+        cancel_future = navigator.goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(navigator, cancel_future)
+        print('Goal was canceled!')
 
 if __name__ == '__main__':
     main()

opennav_coverage_navigator/src/coverage_navigator.cpp

@@ -122,7 +122,7 @@ CoverageNavigator::onLoop()
   try {
     // Get current path points
     nav_msgs::msg::Path current_path;
-    blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);
+    [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path);
 
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =
@@ -165,7 +165,7 @@ CoverageNavigator::onLoop()
   }
 
   int recovery_count = 0;
-  blackboard->get<int>("number_recoveries", recovery_count);
+  [[maybe_unused]] auto result = blackboard->get("number_recoveries", recovery_count);
   feedback_msg->number_of_recoveries = recovery_count;
   feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = clock_->now() - start_time_;