Fixes and Improvements

.github/deps.repos

@@ -3,3 +3,7 @@ repositories:
     type: git
     url: https://github.com/Fields2Cover/Fields2Cover.git
     version: main
+  navigation2:
+    type: git
+    url: https://github.com/ros-planning/navigation2/
+    version: main

.github/workflows/test.yml

@@ -16,7 +16,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [iron]
+          ros_distro: [rolling]
     steps:
     - uses: actions/checkout@v2
     - name: Install Cyclone DDS

.gitignore

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

README.md

@@ -1,6 +1,6 @@
 # Open Navigation's Nav2 Complete Coverage
 
-This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization. It can also compute coverage paths based on open-field polygons **or** based on annotated rows as might exist in a tree farm or other applications with both irregular and regular pre-established rows.
+This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization (topics have Transient Local durability for late-joining visualization tools). It can also compute coverage paths based on open-field polygons **or** based on annotated rows as might exist in a tree farm or other applications with both irregular and regular pre-established rows.
 
 This capability was created by [Open Navigation LLC](https://www.opennav.org/) in partnership with [Bonsai Robotics](https://www.bonsairobotics.ai/). Bonsai Robotics builds autonomous software for machines in adverse and GPS degraded conditions utilizing vision. Bonsai Robotics funded the development of this work for their own product and has graciously allowed Open Navigation to open-source it for the community to leverage in their own systems. Please thank Bonsai Robotics for their commendable donation to the ROS community! Bonsai is hiring [here](https://www.bonsairobotics.ai/careers).
 

opennav_coverage/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(opennav_coverage)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -42,6 +43,7 @@ set(dependencies
   builtin_interfaces
   tf2_ros
   opennav_coverage_msgs
+  ament_index_cpp
 )
 
 add_library(${library_name} SHARED

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/behavior_trees/navigate_w_basic_row_complete_coverage.xml

@@ -11,7 +11,7 @@
   This BT shows field file usage with the row coverage server
 -->
 
-<root main_tree_to_execute="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <RateController hz="0.0000001"> <!-- once, for demo -->
       <Sequence name="NavigateWithoutReplanning">

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"
@@ -125,7 +125,7 @@ TEST_F(CancelCoverageActionTestFixture, test_ports)
 {
   std::string xml_txt =
     R"(
-      <root main_tree_to_execute = "MainTree" >
+      <root BTCPP_format="4" main_tree_to_execute="MainTree" >
         <BehaviorTree ID="MainTree">
              <CancelCoverage name="CoverageCancel"/>
         </BehaviorTree>

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"
@@ -123,7 +123,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
   // create tree
   std::string xml_txt =
     R"(
-      <root main_tree_to_execute = "MainTree" >
+      <root BTCPP_format="4" main_tree_to_execute="MainTree" >
         <BehaviorTree ID="MainTree">
             <ComputeCoveragePath nav_path="{path}"/>
         </BehaviorTree>
@@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
 
   // check if returned path is correct
   nav_msgs::msg::Path path;
-  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  [[maybe_unused]] auto res = config_->blackboard->get("path", path);
   EXPECT_EQ(path.poses.size(), 2u);
   EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
   EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
 
   // halt node so another goal can be sent
-  tree_->rootNode()->halt();
+  tree_->haltTree();
   EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
 }
 

opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py

@@ -150,26 +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:
+    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!')
-    elif result == TaskResult.FAILED:
-        print('Goal failed!')
-    else:
-        print('Goal has an invalid return status!')
 
 
 if __name__ == '__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_;