diff --git a/.github/deps.repos b/.github/deps.repos index ef88206..f2e871a 100644 --- a/.github/deps.repos +++ b/.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 \ No newline at end of file diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 7e2a3e6..e7188ed 100644 --- a/.github/workflows/test.yml +++ b/.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 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/README.md b/README.md index 5503441..3080a0b 100644 --- a/README.md +++ b/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). diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 2e1ff4e..2ffaf99 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/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 diff --git a/opennav_coverage/include/opennav_coverage/visualizer.hpp b/opennav_coverage/include/opennav_coverage/visualizer.hpp index 67afd8f..5e30638 100644 --- a/opennav_coverage/include/opennav_coverage/visualizer.hpp +++ b/opennav_coverage/include/opennav_coverage/visualizer.hpp @@ -46,16 +46,16 @@ class Visualizer { nav_plan_pub_ = rclcpp::create_publisher( 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( 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( 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( node->get_node_topics_interface(), - "coverage_server/swaths", rclcpp::QoS(1)); + "coverage_server/swaths", rclcpp::QoS(1).transient_local()); } void deactivate(); diff --git a/opennav_coverage/src/visualizer.cpp b/opennav_coverage/src/visualizer.cpp index e3b47c9..e98bdd3 100644 --- a/opennav_coverage/src/visualizer.cpp +++ b/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)); } diff --git a/opennav_coverage_bt/CMakeLists.txt b/opennav_coverage_bt/CMakeLists.txt index 7844ee6..3d3aade 100644 --- a/opennav_coverage_bt/CMakeLists.txt +++ b/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) 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 7a2fc52..7743d92 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 @@ -11,7 +11,7 @@ This BT shows field file usage with the row coverage server --> - + diff --git a/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp b/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp index dee58a2..1f9eabd 100644 --- a/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp +++ b/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp @@ -16,7 +16,7 @@ #define OPENNAV_COVERAGE_BT__UTILS_HPP_ #include -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace BT { diff --git a/opennav_coverage_bt/package.xml b/opennav_coverage_bt/package.xml index e8c96db..f207c3b 100644 --- a/opennav_coverage_bt/package.xml +++ b/opennav_coverage_bt/package.xml @@ -18,7 +18,7 @@ nav_msgs geometry_msgs opennav_coverage_msgs - behaviortree_cpp_v3 + behaviortree_cpp ament_lint_common ament_lint_auto diff --git a/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp b/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp index f0658ca..9eee337 100644 --- a/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp +++ b/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 = diff --git a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp index 307aca1..6f988e2 100644 --- a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp +++ b/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 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 = diff --git a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp index 8ffb87c..d6fe48d 100644 --- a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp +++ b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp @@ -17,7 +17,7 @@ #include #include -#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"( - + diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index 09ba0b8..ed79d4c 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/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"( - + @@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("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); } diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py index 5435451..fe4d020 100644 --- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py +++ b/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__': diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index d4e46a7..952dd93 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/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(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("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_;