diff --git a/.github/deps.repos b/.github/deps.repos index ef88206b..f2e871a9 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 7e2a3e6e..e7188ed1 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 00000000..c18dd8d8 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/README.md b/README.md index 55034413..3080a0b2 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 2e1ff4e9..2ffaf99c 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 67afd8f5..5e306380 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 e3b47c94..e98bdd3b 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 7844ee60..3d3aaded 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 7a2fc52d..7743d92e 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 dee58a2a..1f9eabd5 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 e8c96dba..f207c3b1 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 f0658ca2..9eee3379 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 307aca1f..6f988e2f 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 8ffb87ce..d6fe48d3 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 09ba0b8f..ed79d4cb 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 54354511..fe4d020a 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 d4e46a7c..952dd933 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_;