From c81ba020ba450f5f4b02a023f3b8e4586358b4ea Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 09:33:31 +0000 Subject: [PATCH 01/72] Fix BT.CPP upgrade --- opennav_coverage_bt/CMakeLists.txt | 4 ++-- opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp | 2 +- opennav_coverage_bt/package.xml | 2 +- opennav_coverage_bt/src/cancel_complete_coverage_path.cpp | 2 +- opennav_coverage_bt/src/compute_complete_coverage_path.cpp | 2 +- opennav_coverage_bt/test/test_cancel_complete_coverage.cpp | 2 +- opennav_coverage_bt/test/test_compute_coverage_path.cpp | 2 +- opennav_coverage_navigator/src/coverage_navigator.cpp | 4 ++-- 8 files changed, 10 insertions(+), 10 deletions(-) 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/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..6ce5673 100644 --- a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp +++ b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp @@ -98,7 +98,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..630fe3b 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" diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index 09ba0b8..9d21fcc 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" 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_; From 2688106497a7a6f4eb0ca529546fa1d5d2fbdf36 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 16:58:03 +0000 Subject: [PATCH 02/72] Bug fixes --- .gitignore | 1 + .../include/opennav_coverage/visualizer.hpp | 8 ++-- opennav_coverage/src/visualizer.cpp | 1 + .../src/compute_complete_coverage_path.cpp | 1 + .../opennav_coverage_demo/demo_coverage.py | 46 +++++++++++-------- 5 files changed, 33 insertions(+), 24 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ 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..4427d4b 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/src/compute_complete_coverage_path.cpp b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp index 6ce5673..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++) { diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py index 5435451..3cead0c 100644 --- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py +++ b/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() From 0841d1d53f18728bec00fffd6d69bc0cdf35dd01 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 5 Mar 2024 10:23:55 -0800 Subject: [PATCH 03/72] Update opennav_coverage/src/visualizer.cpp --- opennav_coverage/src/visualizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage/src/visualizer.cpp b/opennav_coverage/src/visualizer.cpp index 4427d4b..e98bdd3 100644 --- a/opennav_coverage/src/visualizer.cpp +++ b/opennav_coverage/src/visualizer.cpp @@ -46,7 +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 + utm_path->poses[i].header.frame_id = GLOBAL_FRAME; // Necessary for Foxglove } nav_plan_pub_->publish(std::move(utm_path)); } From 4cbdcc7d0886a252151151b1b30d84274291340a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 19:30:21 +0000 Subject: [PATCH 04/72] add readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 283796a..be6f1e2 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/jobs). From 06497c7ca984b606a040d4a4ce8ef73f93faea33 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 21:03:04 +0000 Subject: [PATCH 05/72] fix flake8 --- opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py index 3cead0c..fe4d020 100644 --- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py +++ b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py @@ -173,10 +173,11 @@ def main(): print('Goal has an invalid return status!') except KeyboardInterrupt: - print("\nCtrl-C detected. Cancelling goal") + 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() From 62d628aae20e9dc3f50fe0cc6089cca4e23d1fe2 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 21:38:01 +0000 Subject: [PATCH 06/72] building fixes --- opennav_coverage/CMakeLists.txt | 2 ++ opennav_coverage_bt/test/test_compute_coverage_path.cpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) 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_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index 9d21fcc..e206778 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -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_->rootNode()->haltNode(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } From 3669e7d84d3065b0a149a20babe35fe5eba008b8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 21:52:30 +0000 Subject: [PATCH 07/72] testing with rolling --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From a3948ec32f25736508b81b142b647f4e8a3401c7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 21:57:05 +0000 Subject: [PATCH 08/72] Revert "testing with rolling" This reverts commit 3669e7d84d3065b0a149a20babe35fe5eba008b8. --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e7188ed..7e2a3e6 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -16,7 +16,7 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling] + ros_distro: [iron] steps: - uses: actions/checkout@v2 - name: Install Cyclone DDS From 2d4810578060d155837ee12d61c985767a7e9ebf Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 6 Mar 2024 16:37:21 +0000 Subject: [PATCH 09/72] test with rolling --- .github/deps.repos | 4 ++++ .github/workflows/test.yml | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) 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 From 99eadcfd6a2eab9cb04266c702df857a022f0641 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 7 Mar 2024 22:01:57 +0000 Subject: [PATCH 10/72] Add BTCPP_format="4" to trees --- .../behavior_trees/navigate_w_basic_row_complete_coverage.xml | 2 +- opennav_coverage_bt/test/test_cancel_complete_coverage.cpp | 2 +- opennav_coverage_bt/test/test_compute_coverage_path.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp index 630fe3b..d6fe48d 100644 --- a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp +++ b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp @@ -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 e206778..20fb146 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -123,7 +123,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + From aa2cf204ebceb08251c2cc26700bba21e78a3758 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 7 Mar 2024 22:06:30 +0000 Subject: [PATCH 11/72] Use tree_->haltTree(); --- opennav_coverage_bt/test/test_compute_coverage_path.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index 20fb146..ed79d4c 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -147,7 +147,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->haltNode(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } From 668abc4318404d3d0ccce9a0aa92e08b3f089aaf Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 14 Mar 2024 08:52:32 +0000 Subject: [PATCH 12/72] Revert "Use tree_->haltTree();" This reverts commit aa2cf204ebceb08251c2cc26700bba21e78a3758. --- opennav_coverage_bt/test/test_compute_coverage_path.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index ed79d4c..20fb146 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -147,7 +147,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->haltTree(); + tree_->rootNode()->haltNode(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } From 1934a78577f9f983ee71e18b9ae07506044de3c4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 14 Mar 2024 08:52:35 +0000 Subject: [PATCH 13/72] Revert "Revert "Use tree_->haltTree();"" This reverts commit 668abc4318404d3d0ccce9a0aa92e08b3f089aaf. --- opennav_coverage_bt/test/test_compute_coverage_path.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index 20fb146..ed79d4c 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -147,7 +147,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->haltNode(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } From 4767882f5484a0fe7592631c7ecb90145e390f83 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Wed, 24 Apr 2024 15:49:32 +1000 Subject: [PATCH 14/72] Update to fields2Cover v2.0.0 api Signed-off-by: Mike Wake --- .../include/opennav_coverage/robot_params.hpp | 14 ++--- .../include/opennav_coverage/utils.hpp | 54 +++++++++---------- opennav_coverage/src/coverage_server.cpp | 8 +-- opennav_coverage/src/path_generator.cpp | 8 ++- opennav_coverage/src/swath_generator.cpp | 2 +- opennav_coverage/test/test_headland.cpp | 4 +- opennav_coverage/test/test_path.cpp | 2 +- opennav_coverage/test/test_robot.cpp | 2 +- opennav_coverage/test/test_route.cpp | 2 +- opennav_coverage/test/test_swath.cpp | 6 +-- opennav_coverage/test/test_utils.cpp | 48 ++++++++--------- 11 files changed, 74 insertions(+), 76 deletions(-) diff --git a/opennav_coverage/include/opennav_coverage/robot_params.hpp b/opennav_coverage/include/opennav_coverage/robot_params.hpp index 35ba0a2..10078ce 100644 --- a/opennav_coverage/include/opennav_coverage/robot_params.hpp +++ b/opennav_coverage/include/opennav_coverage/robot_params.hpp @@ -44,19 +44,19 @@ class RobotParams { nav2_util::declare_parameter_if_not_declared( node, "robot_width", rclcpp::ParameterValue(2.1)); - robot_.robot_width = node->get_parameter("robot_width").as_double(); + robot_.setWidth(node->get_parameter("robot_width").as_double()); nav2_util::declare_parameter_if_not_declared( node, "operation_width", rclcpp::ParameterValue(2.5)); - robot_.op_width = node->get_parameter("operation_width").as_double(); + robot_.setCovWidth(node->get_parameter("operation_width").as_double()); nav2_util::declare_parameter_if_not_declared( node, "min_turning_radius", rclcpp::ParameterValue(0.4)); - robot_.setMinRadius(node->get_parameter("min_turning_radius").as_double()); + robot_.setMinTurningRadius(node->get_parameter("min_turning_radius").as_double()); nav2_util::declare_parameter_if_not_declared( node, "linear_curv_change", rclcpp::ParameterValue(2.0)); - robot_.linear_curv_change = node->get_parameter("linear_curv_change").as_double(); + robot_.setMaxDiffCurv(node->get_parameter("linear_curv_change").as_double()); } /** @@ -65,16 +65,16 @@ class RobotParams */ double getWidth() { - return robot_.robot_width; + return robot_.getWidth(); } /** - * @brief Get the robot's operational width + * @brief Get the robot's operational/coverage width * @return robot's operational width in m */ double getOperationWidth() { - return robot_.op_width; + return robot_.getCovWidth(); } /** diff --git a/opennav_coverage/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp index 43ead77..92edc63 100644 --- a/opennav_coverage/include/opennav_coverage/utils.hpp +++ b/opennav_coverage/include/opennav_coverage/utils.hpp @@ -128,7 +128,7 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( msg.swaths_ordered = true; msg.header = header; - if (raw_path.states.size() == 0) { + if (raw_path.size() == 0) { return msg; } @@ -142,45 +142,45 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( path.moveTo(field.getRefPoint()); } - PathSectionType curr_state = path.states[0].type; + PathSectionType curr_state = path.getState(0).type; if (curr_state == PathSectionType::SWATH) { - curr_swath_start = path.states[0].point; + curr_swath_start = path.getState(0).point; } else if (curr_state == PathSectionType::TURN) { msg.turns.push_back(nav_msgs::msg::Path()); msg.turns.back().header = header; curr_turn = &msg.turns.back(); } - for (unsigned int i = 0; i != path.states.size(); i++) { - if (curr_state == path.states[i].type && path.states[i].type == PathSectionType::SWATH) { + for (unsigned int i = 0; i != path.size(); i++) { + if (curr_state == path.getState(i).type && path.getState(i).type == PathSectionType::SWATH) { // Continuing swath so... // (1) no action required. - } else if (curr_state == path.states[i].type && path.states[i].type == PathSectionType::TURN) { + } else if (curr_state == path.getState(i).type && path.getState(i).type == PathSectionType::TURN) { // Continuing a turn so... // (1) keep populating - curr_turn->poses.push_back(toMsg(path.states[i])); - } else if (curr_state != path.states[i].type && path.states[i].type == PathSectionType::TURN) { + curr_turn->poses.push_back(toMsg(path.getState(i))); + } else if (curr_state != path.getState(i).type && path.getState(i).type == PathSectionType::TURN) { // Transitioning from a swath to a turn so... // (1) Complete the existing swath opennav_coverage_msgs::msg::Swath swath; swath.start = toMsg(curr_swath_start); - swath.end = toMsg(path.states[i - 1].point); + swath.end = toMsg(path.getState(i - 1).point); msg.swaths.push_back(swath); // (2) Start a new turn path msg.turns.push_back(nav_msgs::msg::Path()); msg.turns.back().header = header; curr_turn = &msg.turns.back(); - curr_turn->poses.push_back(toMsg(path.states[i])); - } else if (curr_state != path.states[i].type && path.states[i].type == PathSectionType::SWATH) { + curr_turn->poses.push_back(toMsg(path.getState(i))); + } else if (curr_state != path.getState(i).type && path.getState(i).type == PathSectionType::SWATH) { // Transitioning from a turn to a swath so... // (1) Update new swath starting point - curr_swath_start = path.states[i].point; + curr_swath_start = path.getState(i).point; } - curr_state = path.states[i].type; + curr_state = path.getState(i).type; - if (path.states[i].type != PathSectionType::SWATH && - path.states[i].type != PathSectionType::TURN) + if (path.getState(i).type != PathSectionType::SWATH && + path.getState(i).type != PathSectionType::TURN) { throw std::runtime_error("Unknown type of path state detected, cannot obtain path!"); } @@ -189,7 +189,7 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( if (curr_state == PathSectionType::SWATH) { opennav_coverage_msgs::msg::Swath swath; swath.start = toMsg(curr_swath_start); - swath.end = toMsg(path.states.back().point); + swath.end = toMsg(path.back().point); msg.swaths.push_back(swath); } @@ -215,7 +215,7 @@ inline nav_msgs::msg::Path toNavPathMsg( nav_msgs::msg::Path msg; msg.header = header; - if (raw_path.states.size() == 0) { + if (raw_path.size() == 0) { return msg; } @@ -226,15 +226,15 @@ inline nav_msgs::msg::Path toNavPathMsg( path.moveTo(field.getRefPoint()); } - for (unsigned int i = 0; i != path.states.size(); i++) { + for (unsigned int i = 0; i != path.size(); i++) { // Swaths come in pairs of start-end sequentially - if (i > 0 && path.states[i].type == PathSectionType::SWATH && - path.states[i - 1].type == PathSectionType::SWATH) + if (i > 0 && path.getState(i).type == PathSectionType::SWATH && + path.getState(i - 1).type == PathSectionType::SWATH) { - const float & x0 = path.states[i - 1].point.getX(); - const float & y0 = path.states[i - 1].point.getY(); - const float & x1 = path.states[i].point.getX(); - const float & y1 = path.states[i].point.getY(); + const float & x0 = path.getState(i - 1).point.getX(); + const float & y0 = path.getState(i - 1).point.getY(); + const float & x1 = path.getState(i).point.getX(); + const float & y1 = path.getState(i).point.getY(); const float dist = hypotf(x1 - x0, y1 - y0); const float ux = (x1 - x0) / dist; @@ -243,10 +243,10 @@ inline nav_msgs::msg::Path toNavPathMsg( geometry_msgs::msg::PoseStamped pose; pose.pose.orientation = - nav2_util::geometry_utils::orientationAroundZAxis(path.states[i].angle); + nav2_util::geometry_utils::orientationAroundZAxis(path.getState(i).angle); pose.pose.position.x = x0; pose.pose.position.y = y0; - pose.pose.position.z = path.states[i].point.getZ(); + pose.pose.position.z = path.getState(i).point.getZ(); while (curr_dist < dist) { pose.pose.position.x += pt_dist * ux; @@ -256,7 +256,7 @@ inline nav_msgs::msg::Path toNavPathMsg( } } else { // Turns are already dense paths - msg.poses.push_back(toMsg(path.states[i])); + msg.poses.push_back(toMsg(path.getState(i))); } } diff --git a/opennav_coverage/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp index 0399e71..3481c16 100644 --- a/opennav_coverage/src/coverage_server.cpp +++ b/opennav_coverage/src/coverage_server.cpp @@ -175,7 +175,7 @@ void CoverageServer::computeCoveragePath() std::string frame_id; if (goal->use_gml_file) { master_field = f2c::Parser::importFieldGml(goal->gml_field, true); - frame_id = master_field.coord_sys; + frame_id = master_field.getCRS(); } else { master_field = util::getFieldFromGoal(goal); master_field.setCRS(goal->frame_id); @@ -185,7 +185,7 @@ void CoverageServer::computeCoveragePath() if (!cartesian_frame_) { f2c::Transform::transformToUTM(master_field); } - field = master_field.field.getGeometry(0); + field = master_field.getField().getGeometry(0); RCLCPP_INFO( get_logger(), @@ -269,10 +269,10 @@ CoverageServer::dynamicParametersCallback(std::vector paramet path_gen_->setTurnPointDistance(parameter.as_double()); } else if (name == "robot_width") { auto & robot = robot_params_->getRobot(); - robot.robot_width = parameter.as_double(); + robot.setWidth(parameter.as_double()); } else if (name == "operation_width") { auto & robot = robot_params_->getRobot(); - robot.op_width = parameter.as_double(); + robot.setCovWidth(parameter.as_double()); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == "default_headland_type") { diff --git a/opennav_coverage/src/path_generator.cpp b/opennav_coverage/src/path_generator.cpp index 8febd21..063b938 100644 --- a/opennav_coverage/src/path_generator.cpp +++ b/opennav_coverage/src/path_generator.cpp @@ -26,17 +26,16 @@ Path PathGenerator::generatePath( PathType action_type = toType(settings.mode); PathContinuityType action_continuity_type = toContinuityType(settings.continuity_mode); std::shared_ptr curve{nullptr}; - float turn_point_distance; // If not set by action, use default mode if (action_type == PathType::UNKNOWN || action_continuity_type == PathContinuityType::UNKNOWN) { action_type = default_type_; action_continuity_type = default_continuity_type_; curve = default_curve_; - turn_point_distance = default_turn_point_distance_; + curve->setDiscretization(default_turn_point_distance_); } else { curve = createCurve(action_type, action_continuity_type); - turn_point_distance = settings.turn_point_distance; + curve->setDiscretization(settings.turn_point_distance); } if (!curve) { @@ -46,8 +45,7 @@ Path PathGenerator::generatePath( RCLCPP_DEBUG( logger_, "Generating path with curve: %s", toString(action_type, action_continuity_type).c_str()); - generator_->turn_point_dist = turn_point_distance; - return generator_->searchBestPath(robot_params_->getRobot(), swaths, *curve); + return generator_->planPath(robot_params_->getRobot(), swaths, *curve); } void PathGenerator::setPathMode(const std::string & new_mode) diff --git a/opennav_coverage/src/swath_generator.cpp b/opennav_coverage/src/swath_generator.cpp index 3a1d231..2007577 100644 --- a/opennav_coverage/src/swath_generator.cpp +++ b/opennav_coverage/src/swath_generator.cpp @@ -51,7 +51,7 @@ Swaths SwathGenerator::generateSwaths( if (!objective) { throw CoverageException("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE."); } - generator_->step_angle = step_angle; + generator_->setStepAngle(step_angle); return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field); case SwathAngleType::SET_ANGLE: return generator_->generateSwaths(swath_angle, robot_params_->getOperationWidth(), field); diff --git a/opennav_coverage/test/test_headland.cpp b/opennav_coverage/test/test_headland.cpp index 84b928c..562bcb6 100644 --- a/opennav_coverage/test/test_headland.cpp +++ b/opennav_coverage/test/test_headland.cpp @@ -84,9 +84,9 @@ TEST(HeadlandTests, TestheadlandGeneration) // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::HeadlandMode settings; - auto field_out = generator.generateHeadlands(field.field.getGeometry(0), settings); + auto field_out = generator.generateHeadlands(field.getField().getGeometry(0), settings); settings.mode = "CONSTANT"; - auto field_out2 = generator.generateHeadlands(field.field.getGeometry(0), settings); + auto field_out2 = generator.generateHeadlands(field.getField().getGeometry(0), settings); } } // namespace opennav_coverage diff --git a/opennav_coverage/test/test_path.cpp b/opennav_coverage/test/test_path.cpp index e0b7563..6bec7bf 100644 --- a/opennav_coverage/test/test_path.cpp +++ b/opennav_coverage/test/test_path.cpp @@ -116,7 +116,7 @@ TEST(PathTests, TestpathGeneration) f2c::Random rand; auto field = rand.generateRandField(5, 1e5); opennav_coverage_msgs::msg::SwathMode sw_settings; - auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings); + auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings); opennav_coverage_msgs::msg::RouteMode rt_settings; auto route = route_gen.generateRoute(swaths, rt_settings); diff --git a/opennav_coverage/test/test_robot.cpp b/opennav_coverage/test/test_robot.cpp index 227f600..6905e21 100644 --- a/opennav_coverage/test/test_robot.cpp +++ b/opennav_coverage/test/test_robot.cpp @@ -37,7 +37,7 @@ TEST(RobotTests, Testrobot) EXPECT_EQ(robot.getWidth(), 2.1); EXPECT_EQ(robot.getOperationWidth(), 2.5); - EXPECT_EQ(robot.getRobot().linear_curv_change, 2.0); + EXPECT_EQ(robot.getRobot().getMaxDiffCurv(), 2.0); } } // namespace opennav_coverage diff --git a/opennav_coverage/test/test_route.cpp b/opennav_coverage/test/test_route.cpp index ac11d3e..a4a06ff 100644 --- a/opennav_coverage/test/test_route.cpp +++ b/opennav_coverage/test/test_route.cpp @@ -100,7 +100,7 @@ TEST(RouteTests, TestrouteGeneration) // Generate some toy field f2c::Random rand; auto field = rand.generateRandField(5, 1e5); - auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings); + auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings); // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::RouteMode settings; diff --git a/opennav_coverage/test/test_swath.cpp b/opennav_coverage/test/test_swath.cpp index 997cec1..62bf47b 100644 --- a/opennav_coverage/test/test_swath.cpp +++ b/opennav_coverage/test/test_swath.cpp @@ -112,13 +112,13 @@ TEST(SwathTests, TestswathGeneration) // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::SwathMode settings; - auto swaths1 = generator.generateSwaths(field.field.getGeometry(0), settings); + auto swaths1 = generator.generateSwaths(field.getField().getGeometry(0), settings); settings.mode = "BRUTE_FORCE"; settings.objective = "LENGTH"; - auto swaths2 = generator.generateSwaths(field.field.getGeometry(0), settings); + auto swaths2 = generator.generateSwaths(field.getField().getGeometry(0), settings); settings.mode = "SET_ANGLE"; settings.objective = "NUMBER"; - auto swaths3 = generator.generateSwaths(field.field.getGeometry(0), settings); + auto swaths3 = generator.generateSwaths(field.getField().getGeometry(0), settings); } } // namespace opennav_coverage diff --git a/opennav_coverage/test/test_utils.cpp b/opennav_coverage/test/test_utils.cpp index fd8b63f..5ab5bee 100644 --- a/opennav_coverage/test/test_utils.cpp +++ b/opennav_coverage/test/test_utils.cpp @@ -105,7 +105,7 @@ TEST(UtilsTests, TesttoNavPathMsg) std_msgs::msg::Header header_in; header_in.frame_id = "test"; Path path_in; - path_in.states.resize(10); + path_in.getStates().resize(10); F2CField field; auto msg = util::toNavPathMsg(path_in, field, header_in, true, 0.1); @@ -127,37 +127,37 @@ TEST(UtilsTests, TesttoCoveragePathMsg2) EXPECT_EQ(msg.swaths_ordered, true); // states are not valid (e.g. non-marked as turn or swath) - path_in.states.resize(10); + path_in.getStates().resize(10); EXPECT_THROW(util::toCoveragePathMsg(path_in, field, header_in, true), std::runtime_error); // Now lets make it valid using f2c::types::PathSectionType; - path_in.states[0].type = PathSectionType::SWATH; - path_in.states[1].type = PathSectionType::SWATH; - path_in.states[2].type = PathSectionType::TURN; - path_in.states[3].type = PathSectionType::TURN; - path_in.states[4].type = PathSectionType::SWATH; - path_in.states[5].type = PathSectionType::SWATH; - path_in.states[6].type = PathSectionType::TURN; - path_in.states[7].type = PathSectionType::TURN; - path_in.states[8].type = PathSectionType::SWATH; - path_in.states[9].type = PathSectionType::SWATH; + path_in.getState(0).type = PathSectionType::SWATH; + path_in.getState(1).type = PathSectionType::SWATH; + path_in.getState(2).type = PathSectionType::TURN; + path_in.getState(3).type = PathSectionType::TURN; + path_in.getState(4).type = PathSectionType::SWATH; + path_in.getState(5).type = PathSectionType::SWATH; + path_in.getState(6).type = PathSectionType::TURN; + path_in.getState(7).type = PathSectionType::TURN; + path_in.getState(8).type = PathSectionType::SWATH; + path_in.getState(9).type = PathSectionType::SWATH; msg = util::toCoveragePathMsg(path_in, field, header_in, true); EXPECT_EQ(msg.swaths.size(), 3u); EXPECT_EQ(msg.turns.size(), 2u); EXPECT_EQ(msg.turns[0].poses.size(), 2u); - path_in.states[0].type = PathSectionType::TURN; - path_in.states[1].type = PathSectionType::TURN; - path_in.states[2].type = PathSectionType::SWATH; - path_in.states[3].type = PathSectionType::SWATH; - path_in.states[4].type = PathSectionType::TURN; - path_in.states[5].type = PathSectionType::TURN; - path_in.states[6].type = PathSectionType::SWATH; - path_in.states[7].type = PathSectionType::SWATH; - path_in.states[8].type = PathSectionType::TURN; - path_in.states[9].type = PathSectionType::TURN; + path_in.getState(0).type = PathSectionType::TURN; + path_in.getState(1).type = PathSectionType::TURN; + path_in.getState(2).type = PathSectionType::SWATH; + path_in.getState(3).type = PathSectionType::SWATH; + path_in.getState(4).type = PathSectionType::TURN; + path_in.getState(5).type = PathSectionType::TURN; + path_in.getState(6).type = PathSectionType::SWATH; + path_in.getState(7).type = PathSectionType::SWATH; + path_in.getState(8).type = PathSectionType::TURN; + path_in.getState(9).type = PathSectionType::TURN; msg = util::toCoveragePathMsg(path_in, field, header_in, true); EXPECT_EQ(msg.swaths.size(), 2u); @@ -181,7 +181,7 @@ TEST(UtilsTests, TestgetFieldFromGoal) // Should work now, with a trivial polygon of 3 nodes of (0, 0) goal->polygons[0].coordinates[2].axis1 = 0.0; auto field = util::getFieldFromGoal(goal); - EXPECT_EQ(field.field.getGeometry(0).getGeometry(0).size(), 3u); + EXPECT_EQ(field.getField().getGeometry(0).getGeometry(0).size(), 3u); // Test with inner polygons, first invalid goal->polygons.resize(2); @@ -193,7 +193,7 @@ TEST(UtilsTests, TestgetFieldFromGoal) goal->polygons[0].coordinates[0].axis1 = 0.0; goal->polygons[1].coordinates[0].axis1 = 1.0; auto field2 = util::getFieldFromGoal(goal); - EXPECT_EQ(field2.field.getGeometry(0).getGeometry(1).size(), 3u); + EXPECT_EQ(field2.getField().getGeometry(0).getGeometry(1).size(), 3u); } TEST(UtilsTests, TestPathComponentsIterator) From 9c34271c6d112c2c641b9744fa6e2301a4afa6a6 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 29 Apr 2024 10:10:47 +1000 Subject: [PATCH 15/72] Add dependency on ortools_vendor --- .github/deps.repos | 6 +++++- opennav_coverage/CMakeLists.txt | 1 + opennav_coverage/package.xml | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/deps.repos b/.github/deps.repos index f2e871a..587abcc 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -3,7 +3,11 @@ repositories: type: git url: https://github.com/Fields2Cover/Fields2Cover.git version: main + ortools_vendor: + type: git + url: https://github.com/Fields2Cover/ortools_vendor.git + version: main navigation2: type: git url: https://github.com/ros-planning/navigation2/ - version: main \ No newline at end of file + version: main diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 2ffaf99..289acfe 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/opennav_coverage/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(builtin_interfaces REQUIRED) find_package(tf2_ros REQUIRED) find_package(opennav_coverage_msgs REQUIRED) find_package(Fields2Cover REQUIRED) +find_package(ortools_vendor REQUIRED) # potentially replace with nav2_common, nav2_package() set(CMAKE_CXX_STANDARD 17) diff --git a/opennav_coverage/package.xml b/opennav_coverage/package.xml index 93f41c7..07b8154 100644 --- a/opennav_coverage/package.xml +++ b/opennav_coverage/package.xml @@ -19,6 +19,7 @@ builtin_interfaces tf2_ros fields2cover + ortools_vendor opennav_coverage_msgs ament_lint_common From 4c401d075a00cd8fafc4767a1ceb72873d876c5f Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 29 Apr 2024 13:24:10 +1000 Subject: [PATCH 16/72] Port opennav_row_coverage to fields2cover v2 Signed-off-by: Mike Wake --- opennav_row_coverage/CMakeLists.txt | 6 +++--- opennav_row_coverage/package.xml | 2 +- opennav_row_coverage/src/row_coverage_server.cpp | 8 ++++---- opennav_row_coverage/src/row_swath_generator.cpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt index 2125c62..015bd0e 100644 --- a/opennav_row_coverage/CMakeLists.txt +++ b/opennav_row_coverage/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(nav2_util REQUIRED) find_package(Fields2Cover REQUIRED) find_package(opennav_coverage REQUIRED) find_package(opennav_coverage_msgs REQUIRED) -find_package(tinyxml2 REQUIRED) +find_package(tinyxml2_vendor REQUIRED) # potentially replace with nav2_common, nav2_package() set(CMAKE_CXX_STANDARD 17) @@ -35,6 +35,8 @@ set(dependencies std_msgs opennav_coverage opennav_coverage_msgs + tinyxml2_vendor + #Fields2Cover not packaged with ament ) add_library(${library_name} SHARED @@ -58,8 +60,6 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -target_link_libraries(${executable_name} Fields2Cover tinyxml2::tinyxml2) - rclcpp_components_register_nodes(${library_name} "opennav_row_coverage::RowCoverageServer") install(TARGETS ${library_name} diff --git a/opennav_row_coverage/package.xml b/opennav_row_coverage/package.xml index 9538a82..d30d9ba 100644 --- a/opennav_row_coverage/package.xml +++ b/opennav_row_coverage/package.xml @@ -14,7 +14,7 @@ fields2cover opennav_coverage opennav_coverage_msgs - tinyxml2 + tinyxml2_vendor ament_lint_auto ament_lint_common diff --git a/opennav_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp index 9533013..dc6839e 100644 --- a/opennav_row_coverage/src/row_coverage_server.cpp +++ b/opennav_row_coverage/src/row_coverage_server.cpp @@ -180,7 +180,7 @@ void RowCoverageServer::computeCoveragePath() // (0) Obtain field and rows to use F2CField master_field = f2c::Parser::importFieldGml(goal->gml_field, true); Rows rows = util::parseRows(goal->gml_field, order_ids_); - std::string frame_id = master_field.coord_sys; + std::string frame_id = master_field.getCRS(); if (!cartesian_frame_) { f2c::Transform::transformToUTM(master_field); @@ -188,7 +188,7 @@ void RowCoverageServer::computeCoveragePath() } else { rows = util::removeRowsRefPoint(rows, master_field); } - Field field = master_field.field.getGeometry(0); + Field field = master_field.getField().getGeometry(0); RCLCPP_INFO( get_logger(), @@ -263,10 +263,10 @@ RowCoverageServer::dynamicParametersCallback(std::vector para path_gen_->setTurnPointDistance(parameter.as_double()); } else if (name == "robot_width") { auto & robot = robot_params_->getRobot(); - robot.robot_width = parameter.as_double(); + robot.setWidth(parameter.as_double()); } else if (name == "operation_width") { auto & robot = robot_params_->getRobot(); - robot.op_width = parameter.as_double(); + robot.setCovWidth(parameter.as_double()); } else if (name == "default_offset") { swath_gen_->setOffset(parameter.as_double()); } diff --git a/opennav_row_coverage/src/row_swath_generator.cpp b/opennav_row_coverage/src/row_swath_generator.cpp index c3af7e5..4ec50c3 100644 --- a/opennav_row_coverage/src/row_swath_generator.cpp +++ b/opennav_row_coverage/src/row_swath_generator.cpp @@ -176,7 +176,7 @@ Rows RowSwathGenerator::adjustRowOrientations(const Rows & input_rows) Rows rows = input_rows; auto unitVectorize = [&](const Row & row) { - Point v = row.first.EndPoint() - row.first.StartPoint(); + Point v = row.first.endPoint() - row.first.startPoint(); double normalize = 1.0 / std::sqrt( v.getX() * v.getX() + v.getY() * v.getY()); return v * normalize; From e2d89e10fcf5f7de12e890caea882bdc32a1838c Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 29 Apr 2024 13:54:53 +1000 Subject: [PATCH 17/72] opennav_coverage: workaround non ament packaged Fields2Cover Use tinyxml2_vendor properly. Like in ros/pluginlib IOW first find_package(tinyxml2_vendor REQUIRED) which sets up a module path that provides a module for find_package(TinyXML2 REQUIRED) to work Signed-off-by: Mike Wake --- opennav_row_coverage/CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt index 015bd0e..bf313e9 100644 --- a/opennav_row_coverage/CMakeLists.txt +++ b/opennav_row_coverage/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(Fields2Cover REQUIRED) find_package(opennav_coverage REQUIRED) find_package(opennav_coverage_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # potentially replace with nav2_common, nav2_package() set(CMAKE_CXX_STANDARD 17) @@ -36,7 +37,6 @@ set(dependencies opennav_coverage opennav_coverage_msgs tinyxml2_vendor - #Fields2Cover not packaged with ament ) add_library(${library_name} SHARED @@ -48,7 +48,11 @@ ament_target_dependencies(${library_name} ${dependencies} ) -target_link_libraries(${library_name} Fields2Cover tinyxml2::tinyxml2) +#Fields2Cover not packaged with ament so have to do it manually +#and grab the vendored TINYXML2_LIBRARY too +target_link_libraries(${library_name} + Fields2Cover + tinyxml2::tinyxml2) add_executable(${executable_name} src/main.cpp From 35b0ce5cbaaddad2cec4be72b9edac709023abbd Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 29 Apr 2024 15:41:23 +1000 Subject: [PATCH 18/72] opennav_coverage - fix tests for fields2cover v2 * fields2cover now initialises memory and provides getters/setters so previous incorrectly/partially initialised data no longer throws * Order of args for f2c::Random::generateRandField() changed to (area, sides), was (sides, area). Tests now don't spin forever Signed-off-by: Mike Wake --- opennav_coverage/test/test_headland.cpp | 4 +++- opennav_coverage/test/test_path.cpp | 4 +++- opennav_coverage/test/test_route.cpp | 4 +++- opennav_coverage/test/test_swath.cpp | 4 +++- opennav_coverage/test/test_utils.cpp | 10 +++++++--- opennav_row_coverage/test/test_server.cpp | 2 +- 6 files changed, 20 insertions(+), 8 deletions(-) diff --git a/opennav_coverage/test/test_headland.cpp b/opennav_coverage/test/test_headland.cpp index 562bcb6..119bbb2 100644 --- a/opennav_coverage/test/test_headland.cpp +++ b/opennav_coverage/test/test_headland.cpp @@ -80,7 +80,9 @@ TEST(HeadlandTests, TestheadlandGeneration) // Generate some toy field f2c::Random rand; - auto field = rand.generateRandField(5, 1e5); + double area = 1e5; + int sides = 5; + auto field = rand.generateRandField(area, sides); // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::HeadlandMode settings; diff --git a/opennav_coverage/test/test_path.cpp b/opennav_coverage/test/test_path.cpp index 6bec7bf..81e5a12 100644 --- a/opennav_coverage/test/test_path.cpp +++ b/opennav_coverage/test/test_path.cpp @@ -114,7 +114,9 @@ TEST(PathTests, TestpathGeneration) // Generate some toy route f2c::Random rand; - auto field = rand.generateRandField(5, 1e5); + double area = 1e5; + int sides = 5; + auto field = rand.generateRandField(area, sides); opennav_coverage_msgs::msg::SwathMode sw_settings; auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings); opennav_coverage_msgs::msg::RouteMode rt_settings; diff --git a/opennav_coverage/test/test_route.cpp b/opennav_coverage/test/test_route.cpp index a4a06ff..71b9894 100644 --- a/opennav_coverage/test/test_route.cpp +++ b/opennav_coverage/test/test_route.cpp @@ -99,7 +99,9 @@ TEST(RouteTests, TestrouteGeneration) // Generate some toy field f2c::Random rand; - auto field = rand.generateRandField(5, 1e5); + double area = 1e5; + int sides = 5; + auto field = rand.generateRandField(area, sides); auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings); // Shouldn't throw, results in valid output diff --git a/opennav_coverage/test/test_swath.cpp b/opennav_coverage/test/test_swath.cpp index 62bf47b..2eab672 100644 --- a/opennav_coverage/test/test_swath.cpp +++ b/opennav_coverage/test/test_swath.cpp @@ -108,7 +108,9 @@ TEST(SwathTests, TestswathGeneration) // Generate some toy field f2c::Random rand; - auto field = rand.generateRandField(5, 1e5); + double area = 1e5; + int sides = 5; + auto field = rand.generateRandField(area, sides); // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::SwathMode settings; diff --git a/opennav_coverage/test/test_utils.cpp b/opennav_coverage/test/test_utils.cpp index 5ab5bee..80a1943 100644 --- a/opennav_coverage/test/test_utils.cpp +++ b/opennav_coverage/test/test_utils.cpp @@ -106,11 +106,14 @@ TEST(UtilsTests, TesttoNavPathMsg) header_in.frame_id = "test"; Path path_in; path_in.getStates().resize(10); + EXPECT_EQ(path_in.size(), 10u); F2CField field; auto msg = util::toNavPathMsg(path_in, field, header_in, true, 0.1); EXPECT_EQ(msg.header.frame_id, "test"); - EXPECT_EQ(msg.poses.size(), 10u); + + // Path goes nowhere so compressed into a dense path + EXPECT_EQ(msg.poses.size(), 1u); } TEST(UtilsTests, TesttoCoveragePathMsg2) @@ -126,9 +129,10 @@ TEST(UtilsTests, TesttoCoveragePathMsg2) EXPECT_EQ(msg.contains_turns, true); EXPECT_EQ(msg.swaths_ordered, true); - // states are not valid (e.g. non-marked as turn or swath) + // states are initalised as a valid swath path_in.getStates().resize(10); - EXPECT_THROW(util::toCoveragePathMsg(path_in, field, header_in, true), std::runtime_error); + msg = util::toCoveragePathMsg(path_in, field, header_in, true); + util::toCoveragePathMsg(path_in, field, header_in, true); // Now lets make it valid using f2c::types::PathSectionType; diff --git a/opennav_row_coverage/test/test_server.cpp b/opennav_row_coverage/test/test_server.cpp index 086f405..061a51c 100644 --- a/opennav_row_coverage/test/test_server.cpp +++ b/opennav_row_coverage/test/test_server.cpp @@ -132,7 +132,7 @@ TEST(ServerTest, testDynamicParams) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("default_turn_point_distance", 0.25), + {rclcpp::Parameter("default_turn_point_distance", 0.25), // Discretization ?? rclcpp::Parameter("robot_width", 1.0), rclcpp::Parameter("operation_width", 1.12), rclcpp::Parameter("default_path_type", std::string("hi")), From c770d45cc1a60de55fa059bed4180bfb63c30083 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Fri, 3 May 2024 17:55:14 +1000 Subject: [PATCH 19/72] Revert addition of ortools_vendor to deps.repos --- .github/deps.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index 587abcc..fd5236f 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -3,10 +3,6 @@ repositories: type: git url: https://github.com/Fields2Cover/Fields2Cover.git version: main - ortools_vendor: - type: git - url: https://github.com/Fields2Cover/ortools_vendor.git - version: main navigation2: type: git url: https://github.com/ros-planning/navigation2/ From a221d67cd1d3bd726c7e4ed08a093d7ea01ddd0e Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Fri, 3 May 2024 17:55:45 +1000 Subject: [PATCH 20/72] Remove unnecessary comment --- opennav_coverage/src/visualizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage/src/visualizer.cpp b/opennav_coverage/src/visualizer.cpp index e98bdd3..3e6d66a 100644 --- a/opennav_coverage/src/visualizer.cpp +++ b/opennav_coverage/src/visualizer.cpp @@ -46,7 +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 + utm_path->poses[i].header.frame_id = GLOBAL_FRAME; } nav_plan_pub_->publish(std::move(utm_path)); } From 9c1ccfb57f0fd02506b2c86fd6439ae2268a5690 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Fri, 3 May 2024 18:09:56 +1000 Subject: [PATCH 21/72] Revert addition of navigation2 to deps.repos --- .github/deps.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index fd5236f..ef88206 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -3,7 +3,3 @@ repositories: type: git url: https://github.com/Fields2Cover/Fields2Cover.git version: main - navigation2: - type: git - url: https://github.com/ros-planning/navigation2/ - version: main From fbbcca11f6e33bf649e2d7dc7b0bfd84ae095b8a Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Fri, 3 May 2024 18:16:06 +1000 Subject: [PATCH 22/72] Revert addition of ortools_vendor --- opennav_coverage/CMakeLists.txt | 1 - opennav_coverage/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 289acfe..2ffaf99 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/opennav_coverage/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(builtin_interfaces REQUIRED) find_package(tf2_ros REQUIRED) find_package(opennav_coverage_msgs REQUIRED) find_package(Fields2Cover REQUIRED) -find_package(ortools_vendor REQUIRED) # potentially replace with nav2_common, nav2_package() set(CMAKE_CXX_STANDARD 17) diff --git a/opennav_coverage/package.xml b/opennav_coverage/package.xml index 07b8154..93f41c7 100644 --- a/opennav_coverage/package.xml +++ b/opennav_coverage/package.xml @@ -19,7 +19,6 @@ builtin_interfaces tf2_ros fields2cover - ortools_vendor opennav_coverage_msgs ament_lint_common From 0590d7dd8f29422e36a23a0963bb56ac6126738e Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Fri, 3 May 2024 23:06:00 +1000 Subject: [PATCH 23/72] uncrustify --- .../include/opennav_coverage/utils.hpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/opennav_coverage/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp index 92edc63..d35ea7c 100644 --- a/opennav_coverage/include/opennav_coverage/utils.hpp +++ b/opennav_coverage/include/opennav_coverage/utils.hpp @@ -152,14 +152,20 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( } for (unsigned int i = 0; i != path.size(); i++) { - if (curr_state == path.getState(i).type && path.getState(i).type == PathSectionType::SWATH) { + if (curr_state == path.getState(i).type && + path.getState(i).type == PathSectionType::SWATH) + { // Continuing swath so... // (1) no action required. - } else if (curr_state == path.getState(i).type && path.getState(i).type == PathSectionType::TURN) { + } else if (curr_state == path.getState(i).type && + path.getState(i).type == PathSectionType::TURN) + { // Continuing a turn so... // (1) keep populating curr_turn->poses.push_back(toMsg(path.getState(i))); - } else if (curr_state != path.getState(i).type && path.getState(i).type == PathSectionType::TURN) { + } else if (curr_state != path.getState(i).type && + path.getState(i).type == PathSectionType::TURN) + { // Transitioning from a swath to a turn so... // (1) Complete the existing swath opennav_coverage_msgs::msg::Swath swath; @@ -171,7 +177,9 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( msg.turns.back().header = header; curr_turn = &msg.turns.back(); curr_turn->poses.push_back(toMsg(path.getState(i))); - } else if (curr_state != path.getState(i).type && path.getState(i).type == PathSectionType::SWATH) { + } else if (curr_state != path.getState(i).type && + path.getState(i).type == PathSectionType::SWATH) + { // Transitioning from a turn to a swath so... // (1) Update new swath starting point curr_swath_start = path.getState(i).point; From 10758ad2b5171d66a61f9c57d3db18a8a2172821 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Fri, 3 May 2024 23:15:26 +1000 Subject: [PATCH 24/72] Remove default_turn_point_distance == Discretization comment --- opennav_row_coverage/test/test_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_row_coverage/test/test_server.cpp b/opennav_row_coverage/test/test_server.cpp index 061a51c..086f405 100644 --- a/opennav_row_coverage/test/test_server.cpp +++ b/opennav_row_coverage/test/test_server.cpp @@ -132,7 +132,7 @@ TEST(ServerTest, testDynamicParams) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("default_turn_point_distance", 0.25), // Discretization ?? + {rclcpp::Parameter("default_turn_point_distance", 0.25), rclcpp::Parameter("robot_width", 1.0), rclcpp::Parameter("operation_width", 1.12), rclcpp::Parameter("default_path_type", std::string("hi")), From 1b0cb2b977476e273ca2dffb37b1606b1b9b1df8 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sat, 4 May 2024 19:59:46 +1000 Subject: [PATCH 25/72] cpplint/uncrustify style line lenght if/else style Use intermediate variables for PathSectionType to keep within line lenght limit requirements. --- .../include/opennav_coverage/utils.hpp | 31 +++++++------------ 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/opennav_coverage/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp index d35ea7c..faad2d5 100644 --- a/opennav_coverage/include/opennav_coverage/utils.hpp +++ b/opennav_coverage/include/opennav_coverage/utils.hpp @@ -142,30 +142,25 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( path.moveTo(field.getRefPoint()); } - PathSectionType curr_state = path.getState(0).type; - if (curr_state == PathSectionType::SWATH) { + PathSectionType curr_section_type = path.getState(0).type; + if (curr_section_type == PathSectionType::SWATH) { curr_swath_start = path.getState(0).point; - } else if (curr_state == PathSectionType::TURN) { + } else if (curr_section_type == PathSectionType::TURN) { msg.turns.push_back(nav_msgs::msg::Path()); msg.turns.back().header = header; curr_turn = &msg.turns.back(); } for (unsigned int i = 0; i != path.size(); i++) { - if (curr_state == path.getState(i).type && - path.getState(i).type == PathSectionType::SWATH) - { + auto idx_type = path.getState(i).type; + if (curr_section_type == idx_type && idx_type == PathSectionType::SWATH) { // Continuing swath so... // (1) no action required. - } else if (curr_state == path.getState(i).type && - path.getState(i).type == PathSectionType::TURN) - { + } else if (curr_section_type == idx_type && idx_type == PathSectionType::TURN) { // Continuing a turn so... // (1) keep populating curr_turn->poses.push_back(toMsg(path.getState(i))); - } else if (curr_state != path.getState(i).type && - path.getState(i).type == PathSectionType::TURN) - { + } else if (curr_section_type != idx_type && idx_type == PathSectionType::TURN) { // Transitioning from a swath to a turn so... // (1) Complete the existing swath opennav_coverage_msgs::msg::Swath swath; @@ -177,24 +172,22 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg( msg.turns.back().header = header; curr_turn = &msg.turns.back(); curr_turn->poses.push_back(toMsg(path.getState(i))); - } else if (curr_state != path.getState(i).type && - path.getState(i).type == PathSectionType::SWATH) - { + } else if (curr_section_type != idx_type && idx_type == PathSectionType::SWATH) { // Transitioning from a turn to a swath so... // (1) Update new swath starting point curr_swath_start = path.getState(i).point; } - curr_state = path.getState(i).type; + curr_section_type = idx_type; - if (path.getState(i).type != PathSectionType::SWATH && - path.getState(i).type != PathSectionType::TURN) + if (idx_type != PathSectionType::SWATH && + idx_type != PathSectionType::TURN) { throw std::runtime_error("Unknown type of path state detected, cannot obtain path!"); } } - if (curr_state == PathSectionType::SWATH) { + if (curr_section_type == PathSectionType::SWATH) { opennav_coverage_msgs::msg::Swath swath; swath.start = toMsg(curr_swath_start); swath.end = toMsg(path.back().point); From 363415d3080aa9f240429041075f1903cfc503b6 Mon Sep 17 00:00:00 2001 From: DeccanLin <167321440+DeccanLin@users.noreply.github.com> Date: Mon, 22 Apr 2024 23:19:16 +0800 Subject: [PATCH 26/72] Add gitignore; Add rviz config; (#51) --- .gitignore | 55 ++ .../launch/coverage_demo_launch.py | 4 +- .../launch/row_coverage_demo_launch.py | 3 +- .../rviz/opennav_coverage_demo.rviz | 703 ++++++++++++++++++ opennav_coverage_demo/setup.py | 1 + 5 files changed, 764 insertions(+), 2 deletions(-) create mode 100644 opennav_coverage_demo/rviz/opennav_coverage_demo.rviz diff --git a/.gitignore b/.gitignore index c18dd8d..21696be 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,56 @@ +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Colcon output +build +log +install + +# Visual Studio Code files +.vscode + +# Eclipse project files +.cproject +.project +.pydevproject + +# Python artifacts __pycache__/ +*.py[cod] +.ipynb_checkpoints + +sphinx_doc/_build + +# CLion artifacts +.idea +cmake-build-debug/ + +# doxygen docs +doc/html/ diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py index 1214c19..1db1a33 100644 --- a/opennav_coverage_demo/launch/coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/coverage_demo_launch.py @@ -63,10 +63,12 @@ def generate_launch_description(): '-R', '0.0', '-P', '0.0', '-Y', '0.0']) # start the visualization + rviz_config = os.path.join(coverage_demo_dir, 'opennav_coverage_demo.rviz') + # print("rviz_config:", rviz_config) rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), - launch_arguments={'namespace': ''}.items()) + launch_arguments={'namespace': '', 'rviz_config': rviz_config}.items()) # start navigation bringup_cmd = IncludeLaunchDescription( diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py index 2161b78..a9ee6b3 100644 --- a/opennav_coverage_demo/launch/row_coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/row_coverage_demo_launch.py @@ -63,10 +63,11 @@ def generate_launch_description(): '-R', '0.0', '-P', '0.0', '-Y', '-1.5708']) # start the visualization + rviz_config = os.path.join(coverage_demo_dir, 'opennav_coverage_demo.rviz') rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), - launch_arguments={'namespace': ''}.items()) + launch_arguments={'namespace': '', 'rviz_config': rviz_config}.items()) # start navigation bringup_cmd = IncludeLaunchDescription( diff --git a/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz b/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz new file mode 100644 index 0000000..2239510 --- /dev/null +++ b/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz @@ -0,0 +1,703 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Controller1 + - /Polygon1 + - /Polygon2 + - /Path1 + - /Path2 + Splitter Ratio: 0.5833333134651184 + Tree Height: 575 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + EPSG:4258: + Value: true + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + wheel_left_link: + Value: true + wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + EPSG:4258: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: -999999 + Min Color: 0; 0; 0 + Min Intensity: 999999 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep All + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /coverage_server/field_boundary + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 255; 85; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /coverage_server/planning_field + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 85; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.07999999821186066 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /coverage_server/coverage_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 170; 127 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.20000000298023224 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /received_global_plan + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -0.004999999888241291 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 47.66288757324219 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 11.359171867370605 + Y: 9.817364692687988 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1072 + Hide Left Dock: false + Hide Right Dock: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c8000003dafc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000027a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002bb0000015a0000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d006500720061000000037e0000009f0000002800ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003da000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000495000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1912 + X: 1082 + Y: 2 diff --git a/opennav_coverage_demo/setup.py b/opennav_coverage_demo/setup.py index b13c308..76d11f8 100644 --- a/opennav_coverage_demo/setup.py +++ b/opennav_coverage_demo/setup.py @@ -17,6 +17,7 @@ (os.path.join('share', package_name), glob('launch/*')), (os.path.join('share', package_name), glob('world/*')), (os.path.join('share', package_name), glob('params/*')), + (os.path.join('share', package_name), glob('rviz/*')), ], install_requires=['setuptools'], zip_safe=True, From 9a38dc4ab8c7ad2a851f8ff04b2fa0c13974bab2 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sat, 4 May 2024 20:37:38 +1000 Subject: [PATCH 27/72] demo rviz coverage_plan/coverage_plan Transient Local --- opennav_coverage_demo/rviz/opennav_coverage_demo.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz b/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz index 2239510..e35f648 100644 --- a/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz +++ b/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz @@ -594,7 +594,7 @@ Visualization Manager: Shaft Length: 0.10000000149011612 Topic: Depth: 5 - Durability Policy: Volatile + Durability Policy: Transient Local Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable From ed382a4c85dcab26c2ca90988da2ca4e6207af04 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 09:25:02 +1000 Subject: [PATCH 28/72] Put navigation2 back into .github/deps.repos for nav_msgs This reverts commit 9c1ccfb57f0fd02506b2c86fd6439ae2268a5690. --- .github/deps.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/deps.repos b/.github/deps.repos index ef88206..fd5236f 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 From d44f82c9ea5b0932f84e95cb26c7ac6429376d1a Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 09:32:38 +1000 Subject: [PATCH 29/72] DO NOT MERGE - temporarily pointing at aosmw fork of Fields2Cover --- .github/deps.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index fd5236f..90dabe5 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -1,8 +1,8 @@ repositories: Fields2Cover: type: git - url: https://github.com/Fields2Cover/Fields2Cover.git - version: main + url: https://github.com/aosmw/Fields2Cover.git + version: feature/mw/ros2_build navigation2: type: git url: https://github.com/ros-planning/navigation2/ From 619b528eac3e28907e97c1fb2d12cf31db0a2cb7 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 10:33:03 +1000 Subject: [PATCH 30/72] ci - attempt workaround rosdep not providing geometry_msgs --- .github/workflows/test.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e7188ed..8017561 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -21,6 +21,10 @@ jobs: - uses: actions/checkout@v2 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + - name: Install geometry_msgs + run: sudo apt install ros-${{ matrix.ros_distro }}-geometry-msgs -y + - name: Install nav_msgs + run: sudo apt install ros-${{ matrix.ros_distro }}-nav-msgs -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 8486ed0505f8a742bf6b050373e4d59df2d98b8b Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 12:51:41 +1000 Subject: [PATCH 31/72] ci - attempt workaround rosdep not providing rosidl_default_generators --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 8017561..73bf825 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -25,6 +25,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-geometry-msgs -y - name: Install nav_msgs run: sudo apt install ros-${{ matrix.ros_distro }}-nav-msgs -y + - name: Install rosidl-default-generators + run: sudo apt install ros-${{ matrix.ros_distro }}-rosidl-default-generators -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From be161dab1947309a5f5164946d99d6edaf447988 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 13:47:55 +1000 Subject: [PATCH 32/72] ci = guess a better base image that already has rclcpp in it --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 73bf825..2694994 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: rostooling/setup-ros-docker:ubuntu-jammy-latest + image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest strategy: fail-fast: false matrix: From ba3a6c93d014c81a7ef64e216b29910cf334f9be Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 14:22:12 +1000 Subject: [PATCH 33/72] ci - install geographic_msgs --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2694994..55fc7c7 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -27,6 +27,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-nav-msgs -y - name: Install rosidl-default-generators run: sudo apt install ros-${{ matrix.ros_distro }}-rosidl-default-generators -y + - name: Install geographic_msgs + run: sudo apt install ros-${{ matrix.ros_distro }}-geographic_msgs -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 4628bf332ac182e22eef8488961a47e02f885be0 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 14:25:08 +1000 Subject: [PATCH 34/72] Fix spelling of geographic_msgs package --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 55fc7c7..7dd4c06 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -28,7 +28,7 @@ jobs: - name: Install rosidl-default-generators run: sudo apt install ros-${{ matrix.ros_distro }}-rosidl-default-generators -y - name: Install geographic_msgs - run: sudo apt install ros-${{ matrix.ros_distro }}-geographic_msgs -y + run: sudo apt install ros-${{ matrix.ros_distro }}-geographic-msgs -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 7649af9d976c76c7e426de7aaaadf646d535195c Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 14:37:12 +1000 Subject: [PATCH 35/72] ci - install bondcpp and angles --- .github/workflows/test.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 7dd4c06..1f20311 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -29,6 +29,10 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-rosidl-default-generators -y - name: Install geographic_msgs run: sudo apt install ros-${{ matrix.ros_distro }}-geographic-msgs -y + - name: Install bondcpp + run: sudo apt install ros-${{ matrix.ros_distro }}-bondcpp -y + - name: Install angles + run: sudo apt install ros-${{ matrix.ros_distro }}-angles -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 3de3c4e13be83bb380d73b541c95e022672a6420 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 14:54:56 +1000 Subject: [PATCH 36/72] Use ros-tooling/action-ros-ci@v0.3.11, rosdep-check: true, add test_msgs --- .github/workflows/test.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 1f20311..a505468 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -33,13 +33,16 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-bondcpp -y - name: Install angles run: sudo apt install ros-${{ matrix.ros_distro }}-angles -y + - name: Install test_msgs + run: sudo apt install ros-${{ matrix.ros_distro }}-test-msgs -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.3.11 with: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" + rosdep-check: true - uses: actions/upload-artifact@v1 with: name: colcon-logs From 47d0890bcc5af5e1c08e4cd78eb36375940239d3 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 15:07:33 +1000 Subject: [PATCH 37/72] ros-tooling/action-ros-ci@v0.3.11 not found go back to v0.3 Error: Unable to resolve action 'ros-tooling/action-ros-ci@v0.3.11', unable to find version v0.3.11' --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index a505468..e9b53f4 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -37,7 +37,7 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-test-msgs -y - name: Build and run tests id: action-ros-ci - uses: ros-tooling/action-ros-ci@v0.3.11 + uses: ros-tooling/action-ros-ci@v0.3 with: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: ${{ matrix.ros_distro }} From 54b19a5d6c23796abb909a529815edba1f82039d Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 15:20:23 +1000 Subject: [PATCH 38/72] ci - install behaviortree_cpp --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e9b53f4..a22f305 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -35,6 +35,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-angles -y - name: Install test_msgs run: sudo apt install ros-${{ matrix.ros_distro }}-test-msgs -y + - name: Install behaviortree_cpp + run: sudo apt install ros-${{ matrix.ros_distro }}-behaviortree-cpp -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 33490eeba5e4adcf539425152b3329ee25e70352 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 15:41:01 +1000 Subject: [PATCH 39/72] Install diagnostic-updater --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index a22f305..d5edcd8 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -37,6 +37,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-test-msgs -y - name: Install behaviortree_cpp run: sudo apt install ros-${{ matrix.ros_distro }}-behaviortree-cpp -y + - name: Install diagnostic_updater + run: sudo apt install ros-${{ matrix.ros_distro }}-diagnostic-updater -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From c30abd3e0a3f1af48c10b4a0a5f87faf07372ba4 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 7 May 2024 16:40:09 +1000 Subject: [PATCH 40/72] Try to get this to work with act(Run github actions locally). https://nektosact.com/introduction.html Run github actions locally. It doesn't work locally for me. If fails on vcs import --force --recursive src/ < package.repo Could not checkout ref 'a077eb600a3700d1053482b3582822fa1b6e48db': fatal: reference is not a tree: a077eb600a3700d1053482b3582822fa1b6e48db --- .github/deps.repos | 2 +- .github/workflows/test.yml | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/deps.repos b/.github/deps.repos index 90dabe5..3352547 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -5,5 +5,5 @@ repositories: version: feature/mw/ros2_build navigation2: type: git - url: https://github.com/ros-planning/navigation2/ + url: https://github.com/ros-planning/navigation2.git version: main diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index d5edcd8..8a9c89b 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -18,6 +18,10 @@ jobs: matrix: ros_distro: [rolling] steps: + - name: apt update + run: sudo apt update + - name: Install nodejs + run: sudo apt install nodejs -y - uses: actions/checkout@v2 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y @@ -39,6 +43,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-behaviortree-cpp -y - name: Install diagnostic_updater run: sudo apt install ros-${{ matrix.ros_distro }}-diagnostic-updater -y + - name: Install rviz_common + run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-common -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From eb1488c4e73a57b4a1eb7c0154b34e75bb48e9a5 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 9 May 2024 09:40:47 +1000 Subject: [PATCH 41/72] More ci dependencies for nav2 build --- .github/workflows/test.yml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 8a9c89b..6030ec4 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -45,6 +45,16 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-diagnostic-updater -y - name: Install rviz_common run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-common -y + - name: Install rviz_default_plugins + run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-default-plugins -y + - name: Install ompl + run: sudo apt install ros-${{ matrix.ros_distro }}-ompl -y + - name: Install laser_geometry + run: sudo apt install ros-${{ matrix.ros_distro }}-laser_geometry -y + - name: Install cv_bridge + run: sudo apt install ros-${{ matrix.ros_distro }}-cv-bridge -y + - name: Install gazebo_ros_pkgs + run: sudo apt install ros-${{ matrix.ros_distro }}-gazebo-ros-pkgs -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 77d1c941a13561b3a22d8abb406f69841f92f9f2 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 9 May 2024 11:08:41 +1000 Subject: [PATCH 42/72] ci - fix laser_geometry package name --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 6030ec4..7578f54 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -50,7 +50,7 @@ jobs: - name: Install ompl run: sudo apt install ros-${{ matrix.ros_distro }}-ompl -y - name: Install laser_geometry - run: sudo apt install ros-${{ matrix.ros_distro }}-laser_geometry -y + run: sudo apt install ros-${{ matrix.ros_distro }}-laser-geometry -y - name: Install cv_bridge run: sudo apt install ros-${{ matrix.ros_distro }}-cv-bridge -y - name: Install gazebo_ros_pkgs From 2db2ce9b0ed7422b38e067a466b57425aebc3720 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 9 May 2024 12:05:01 +1000 Subject: [PATCH 43/72] ci - nav2 contrained smoother wants libceres-dev --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 7578f54..261a89c 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -55,6 +55,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-cv-bridge -y - name: Install gazebo_ros_pkgs run: sudo apt install ros-${{ matrix.ros_distro }}-gazebo-ros-pkgs -y + - name: Install libceres-dev + run: sudo apt install libceres-dev -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From d9c63edb6e0ff3530acdd1ca981e6d4e9f3807aa Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 9 May 2024 12:32:14 +1000 Subject: [PATCH 44/72] ci - try libunwind-dev to try to get libgoogle-glog-dev to be happy for libceres-dev --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 261a89c..e6b0209 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -55,6 +55,8 @@ jobs: run: sudo apt install ros-${{ matrix.ros_distro }}-cv-bridge -y - name: Install gazebo_ros_pkgs run: sudo apt install ros-${{ matrix.ros_distro }}-gazebo-ros-pkgs -y + - name: Install libunwind-dev + run: sudo apt install libunwind-dev -y - name: Install libceres-dev run: sudo apt install libceres-dev -y - name: Build and run tests From e14194f7e7af978801a7fd858c94f5fc11e2b8b3 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 9 May 2024 13:21:06 +1000 Subject: [PATCH 45/72] ci - need robot-localization --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e6b0209..f04d917 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -59,6 +59,8 @@ jobs: run: sudo apt install libunwind-dev -y - name: Install libceres-dev run: sudo apt install libceres-dev -y + - name: Install robot_localization + run: sudo apt install ros-${{ matrix.ros_distro }}-robot-localization -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 6d4e370d88963220ce2e290454ad9fec32728a44 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sun, 12 May 2024 13:20:32 +1000 Subject: [PATCH 46/72] Build and test only what is necessary for this repo --- .github/workflows/test.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index f04d917..db287ff 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -69,6 +69,19 @@ jobs: target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" rosdep-check: true + package-name: "opennav_coverage_demo" + colcon-defaults: | + { + "test": { + "packages-select": [ + "opennav_coverage", + "opennav_coverage_bt", + "opennav_row_coverage", + "opennav_coverage_navigator", + "opennav_coverage_demo" + ] + } + } - uses: actions/upload-artifact@v1 with: name: colcon-logs From 5062803a40531488f4a1caa2f0d222d2fd96505a Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 13 May 2024 10:29:40 +1000 Subject: [PATCH 47/72] Attempt build on noble, remove rosdep-check: true which doesn't perform install --- .github/workflows/test.yml | 51 ++++---------------------------------- 1 file changed, 5 insertions(+), 46 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index db287ff..512bfee 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -6,70 +6,29 @@ on: - main jobs: - build_and_test: + noble_rolling: runs-on: ubuntu-latest env: ROS_DISTRO: ${{ matrix.ros_distro }} RMW_IMPLEMENTATION: rmw_cyclonedds_cpp container: - image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest strategy: fail-fast: false matrix: ros_distro: [rolling] steps: - - name: apt update - run: sudo apt update - - name: Install nodejs - run: sudo apt install nodejs -y - - uses: actions/checkout@v2 + - name: checkout source + uses: actions/checkout@v4 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - name: Install geometry_msgs - run: sudo apt install ros-${{ matrix.ros_distro }}-geometry-msgs -y - - name: Install nav_msgs - run: sudo apt install ros-${{ matrix.ros_distro }}-nav-msgs -y - - name: Install rosidl-default-generators - run: sudo apt install ros-${{ matrix.ros_distro }}-rosidl-default-generators -y - - name: Install geographic_msgs - run: sudo apt install ros-${{ matrix.ros_distro }}-geographic-msgs -y - - name: Install bondcpp - run: sudo apt install ros-${{ matrix.ros_distro }}-bondcpp -y - - name: Install angles - run: sudo apt install ros-${{ matrix.ros_distro }}-angles -y - - name: Install test_msgs - run: sudo apt install ros-${{ matrix.ros_distro }}-test-msgs -y - - name: Install behaviortree_cpp - run: sudo apt install ros-${{ matrix.ros_distro }}-behaviortree-cpp -y - - name: Install diagnostic_updater - run: sudo apt install ros-${{ matrix.ros_distro }}-diagnostic-updater -y - - name: Install rviz_common - run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-common -y - - name: Install rviz_default_plugins - run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-default-plugins -y - - name: Install ompl - run: sudo apt install ros-${{ matrix.ros_distro }}-ompl -y - - name: Install laser_geometry - run: sudo apt install ros-${{ matrix.ros_distro }}-laser-geometry -y - - name: Install cv_bridge - run: sudo apt install ros-${{ matrix.ros_distro }}-cv-bridge -y - - name: Install gazebo_ros_pkgs - run: sudo apt install ros-${{ matrix.ros_distro }}-gazebo-ros-pkgs -y - - name: Install libunwind-dev - run: sudo apt install libunwind-dev -y - - name: Install libceres-dev - run: sudo apt install libceres-dev -y - - name: Install robot_localization - run: sudo apt install ros-${{ matrix.ros_distro }}-robot-localization -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 with: - import-token: ${{ secrets.GITHUB_TOKEN }} + package-name: "opennav_coverage_demo" target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" - rosdep-check: true - package-name: "opennav_coverage_demo" colcon-defaults: | { "test": { From fe41c6b35d9eb5b0a51b6d2222ff0cb7cca2b3d0 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 13 May 2024 10:43:17 +1000 Subject: [PATCH 48/72] Add separate jammy rolling action --- .github/workflows/test.yml | 39 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 512bfee..053e2cb 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -6,6 +6,45 @@ on: - main jobs: + jammy_rolling: + runs-on: ubuntu-latest + env: + ROS_DISTRO: ${{ matrix.ros_distro }} + RMW_IMPLEMENTATION: rmw_cyclonedds_cpp + container: + image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + strategy: + fail-fast: false + matrix: + ros_distro: [rolling] + steps: + - name: checkout source + uses: actions/checkout@v4 + - 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 + with: + package-name: "opennav_coverage_demo" + target-ros2-distro: ${{ matrix.ros_distro }} + vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" + colcon-defaults: | + { + "test": { + "packages-select": [ + "opennav_coverage", + "opennav_coverage_bt", + "opennav_row_coverage", + "opennav_coverage_navigator", + "opennav_coverage_demo" + ] + } + } + - uses: actions/upload-artifact@v1 + with: + name: colcon-logs + path: ros_ws/log noble_rolling: runs-on: ubuntu-latest env: From 6a00d89a20342977c8e377ccb4d94df08b3901c0 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 13 May 2024 14:17:00 +1000 Subject: [PATCH 49/72] Try removing python3-nose --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 053e2cb..58d6b98 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -61,6 +61,8 @@ jobs: uses: actions/checkout@v4 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + - name: Remove python3-nose + run: sudo apt remove python3-nose -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From ffcb2e33a8c8447ccf25c89838320ce710db2c70 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 09:33:24 +1000 Subject: [PATCH 50/72] Try running apt-get update to get latest python3-rosdep image is currently 7 weeks old --- .github/workflows/test.yml | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 58d6b98..2ac8821 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -20,6 +20,12 @@ jobs: steps: - name: checkout source uses: actions/checkout@v4 + - name: run apt-get update + run: sudo apt-get update + - name: run apt-get upgrade --list-upgradable + run: sudo apt-get update --list-upgradeable + - name: run apt upgrade python3-rosdep + run: sudo apt-get upgrade python3-rosdep -y - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Build and run tests @@ -59,10 +65,14 @@ jobs: steps: - name: checkout source uses: actions/checkout@v4 + - name: run apt-get update + run: sudo apt-get update + - name: run apt-get upgrade --list-upgradable + run: sudo apt-get update --list-upgradeable + - name: run apt upgrade python3-rosdep + run: sudo apt-get upgrade python3-rosdep -y - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - name: Remove python3-nose - run: sudo apt remove python3-nose -y - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From ec7ab9d596d57eb43f0b97ac943d6e203e1188d8 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 11:43:34 +1000 Subject: [PATCH 51/72] pip uninstall nose colcon finds nosetests which uses imp which is not supported in python3.12 --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2ac8821..672a742 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -71,6 +71,8 @@ jobs: run: sudo apt-get update --list-upgradeable - name: run apt upgrade python3-rosdep run: sudo apt-get upgrade python3-rosdep -y + - name: Unistall python nose - uses imp not supported python3.12 + run: sudo pip uninstall nose - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Build and run tests From 6499975c7308b6c75fc5d2b9a06994fb2377eafe Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 11:51:16 +1000 Subject: [PATCH 52/72] Fix apt update, use PIP_BREAK_SYSTEM_PACKAGES=1 for pip command --- .github/workflows/test.yml | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 672a742..42d1e08 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -65,14 +65,16 @@ jobs: steps: - name: checkout source uses: actions/checkout@v4 - - name: run apt-get update - run: sudo apt-get update - - name: run apt-get upgrade --list-upgradable - run: sudo apt-get update --list-upgradeable - - name: run apt upgrade python3-rosdep - run: sudo apt-get upgrade python3-rosdep -y + - name: run apt update + run: apt update + - name: run apt list --upgradable + run: apt list --upgradeable + - name: run apt upgrade python3-rosdep -y + run: apt upgrade python3-rosdep -y - name: Unistall python nose - uses imp not supported python3.12 - run: sudo pip uninstall nose + run: pip uninstall nose + env: + PIP_BREAK_SYSTEM_PACKAGES=1 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Build and run tests From d2b9fd0e7bcb353a26e93fadfdd241faf0af571d Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 11:53:35 +1000 Subject: [PATCH 53/72] fix PIP_BREAK_SYSTEM_PACKAGES env var --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 42d1e08..5f69d58 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -74,7 +74,7 @@ jobs: - name: Unistall python nose - uses imp not supported python3.12 run: pip uninstall nose env: - PIP_BREAK_SYSTEM_PACKAGES=1 + PIP_BREAK_SYSTEM_PACKAGES: 1 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Build and run tests From 148c84a15f1a5a1e76de461025e80d122f5e2fec Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 15:46:34 +1000 Subject: [PATCH 54/72] more worflow --- .github/workflows/test.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 5f69d58..fba87f0 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -20,10 +20,10 @@ jobs: steps: - name: checkout source uses: actions/checkout@v4 - - name: run apt-get update - run: sudo apt-get update - - name: run apt-get upgrade --list-upgradable - run: sudo apt-get update --list-upgradeable + - name: run apt update + run: sudo apt update + - name: run apt --list-upgradable + run: sudo apt --list-upgradeable - name: run apt upgrade python3-rosdep run: sudo apt-get upgrade python3-rosdep -y - name: Install Cyclone DDS @@ -98,5 +98,5 @@ jobs: } - uses: actions/upload-artifact@v1 with: - name: colcon-logs + name: noble-colcon-logs path: ros_ws/log From 01d517191d8ec8d06eec9a87bdfce887903037e2 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 15:52:20 +1000 Subject: [PATCH 55/72] workflow --- .github/workflows/test.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index fba87f0..37c59bb 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -21,11 +21,11 @@ jobs: - name: checkout source uses: actions/checkout@v4 - name: run apt update - run: sudo apt update - - name: run apt --list-upgradable - run: sudo apt --list-upgradeable - - name: run apt upgrade python3-rosdep - run: sudo apt-get upgrade python3-rosdep -y + run: apt update + - name: run apt list --upgradable + run: apt list --upgradeable + - name: run apt upgrade python3-rosdep -y + run: apt upgrade python3-rosdep -y - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Build and run tests @@ -72,7 +72,7 @@ jobs: - name: run apt upgrade python3-rosdep -y run: apt upgrade python3-rosdep -y - name: Unistall python nose - uses imp not supported python3.12 - run: pip uninstall nose + run: pip uninstall nose -y env: PIP_BREAK_SYSTEM_PACKAGES: 1 - name: Install Cyclone DDS From f73d34df6022c47e53fbd09873b719d407ffacb1 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 19:12:10 +1000 Subject: [PATCH 56/72] Attempt to manipulate rosdep index for jammy back to Feb 28,2024 --- .github/workflows/test.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 37c59bb..22144c9 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -28,6 +28,8 @@ jobs: run: apt upgrade python3-rosdep -y - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + - name: Temp: Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28, 2024 + run: sed -i "s|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|" /etc/ros/rosdep/sources.list.d/20-default.list - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 @@ -47,6 +49,8 @@ jobs: ] } } + env: + ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml - uses: actions/upload-artifact@v1 with: name: colcon-logs From c4e9359077cf28a99ec210d395aaeadc5b05fddb Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 19:23:30 +1000 Subject: [PATCH 57/72] Try single quotes for sed --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 22144c9..2dc2678 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -29,7 +29,7 @@ jobs: - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Temp: Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28, 2024 - run: sed -i "s|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|" /etc/ros/rosdep/sources.list.d/20-default.list + run: sed -i 's|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|' /etc/ros/rosdep/sources.list.d/20-default.list - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 89821d374664a71a5a821238cb2bd4f6bd8da4b6 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 19:26:43 +1000 Subject: [PATCH 58/72] action syntax --- .github/workflows/test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2dc2678..25feb75 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -28,7 +28,7 @@ jobs: run: apt upgrade python3-rosdep -y - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - name: Temp: Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28, 2024 + - name: Temp Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28 2024 run: sed -i 's|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|' /etc/ros/rosdep/sources.list.d/20-default.list - name: Build and run tests id: action-ros-ci @@ -50,7 +50,7 @@ jobs: } } env: - ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml + ROSDISTRO_INDEX_URL: https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml - uses: actions/upload-artifact@v1 with: name: colcon-logs From ff68d03c2f94de5a3de1f5833f62124d23507d94 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 19:31:30 +1000 Subject: [PATCH 59/72] action syntax sed --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 25feb75..2d5d701 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -29,7 +29,7 @@ jobs: - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - name: Temp Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28 2024 - run: sed -i 's|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|' /etc/ros/rosdep/sources.list.d/20-default.list + run: sed -i 's@ros/rosdistro/master@ros/rosdistro/rolling/2024-02-28@' /etc/ros/rosdep/sources.list.d/20-default.list - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 36b818b0d68b6aba2ef2554c8c0cedda16dce83a Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 19:49:41 +1000 Subject: [PATCH 60/72] Run through yamllint --- .github/workflows/test.yml | 150 +++++++++++++++++++------------------ 1 file changed, 76 insertions(+), 74 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2d5d701..8053f58 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -1,3 +1,4 @@ +--- name: Test on: pull_request: @@ -16,45 +17,46 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling] + ros_distro: [rolling] steps: - - name: checkout source - uses: actions/checkout@v4 - - name: run apt update - run: apt update - - name: run apt list --upgradable - run: apt list --upgradeable - - name: run apt upgrade python3-rosdep -y - run: apt upgrade python3-rosdep -y - - name: Install Cyclone DDS - run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - name: Temp Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28 2024 - run: sed -i 's@ros/rosdistro/master@ros/rosdistro/rolling/2024-02-28@' /etc/ros/rosdep/sources.list.d/20-default.list - - name: Build and run tests - id: action-ros-ci - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: "opennav_coverage_demo" - target-ros2-distro: ${{ matrix.ros_distro }} - vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" - colcon-defaults: | - { - "test": { - "packages-select": [ - "opennav_coverage", - "opennav_coverage_bt", - "opennav_row_coverage", - "opennav_coverage_navigator", - "opennav_coverage_demo" - ] + - name: checkout source + uses: actions/checkout@v4 + - name: run apt update + run: apt update + - name: run apt list --upgradable + run: apt list --upgradeable + - name: run apt upgrade python3-rosdep -y + run: apt upgrade python3-rosdep -y + - name: Install Cyclone DDS + run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + - name: Temp Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28 2024 + run: | + sed -i 's@ros/rosdistro/master@ros/rosdistro/rolling/2024-02-28@' /etc/ros/rosdep/sources.list.d/20-default.list + - name: Build and run tests + id: action-ros-ci + uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: "opennav_coverage_demo" + target-ros2-distro: ${{ matrix.ros_distro }} + vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" + colcon-defaults: | + { + "test": { + "packages-select": [ + "opennav_coverage", + "opennav_coverage_bt", + "opennav_row_coverage", + "opennav_coverage_navigator", + "opennav_coverage_demo" + ] + } } - } - env: - ROSDISTRO_INDEX_URL: https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml - - uses: actions/upload-artifact@v1 - with: - name: colcon-logs - path: ros_ws/log + env: + ROSDISTRO_INDEX_URL: https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml + - uses: actions/upload-artifact@v1 + with: + name: colcon-logs + path: ros_ws/log noble_rolling: runs-on: ubuntu-latest env: @@ -65,42 +67,42 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling] + ros_distro: [rolling] steps: - - name: checkout source - uses: actions/checkout@v4 - - name: run apt update - run: apt update - - name: run apt list --upgradable - run: apt list --upgradeable - - name: run apt upgrade python3-rosdep -y - run: apt upgrade python3-rosdep -y - - name: Unistall python nose - uses imp not supported python3.12 - run: pip uninstall nose -y - env: - PIP_BREAK_SYSTEM_PACKAGES: 1 - - 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 - with: - package-name: "opennav_coverage_demo" - target-ros2-distro: ${{ matrix.ros_distro }} - vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" - colcon-defaults: | - { - "test": { - "packages-select": [ - "opennav_coverage", - "opennav_coverage_bt", - "opennav_row_coverage", - "opennav_coverage_navigator", - "opennav_coverage_demo" - ] + - name: checkout source + uses: actions/checkout@v4 + - name: run apt update + run: apt update + - name: run apt list --upgradable + run: apt list --upgradeable + - name: run apt upgrade python3-rosdep -y + run: apt upgrade python3-rosdep -y + - name: Unistall python nose - uses imp not supported python3.12 + run: pip uninstall nose -y + env: + PIP_BREAK_SYSTEM_PACKAGES: 1 + - 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 + with: + package-name: "opennav_coverage_demo" + target-ros2-distro: ${{ matrix.ros_distro }} + vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" + colcon-defaults: | + { + "test": { + "packages-select": [ + "opennav_coverage", + "opennav_coverage_bt", + "opennav_row_coverage", + "opennav_coverage_navigator", + "opennav_coverage_demo" + ] + } } - } - - uses: actions/upload-artifact@v1 - with: - name: noble-colcon-logs - path: ros_ws/log + - uses: actions/upload-artifact@v1 + with: + name: noble-colcon-logs + path: ros_ws/log From c9d4bf405aad285f41603cfe287f59c29f7955df Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 20:14:35 +1000 Subject: [PATCH 61/72] Adjust noble_rolling to use pre-release binaries This is so that nav2 can build with -Werror against behaviortree_cpp v4.6 --- .github/workflows/test.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 8053f58..e8c6d94 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -69,6 +69,10 @@ jobs: matrix: ros_distro: [rolling] steps: + - name: Use pre-release binaries from ros2-testing repository - v4.6 behaviortree_cpp + uses: ros-tooling/setup-ros@v0.7 + with: + use-ros2-testing: true - name: checkout source uses: actions/checkout@v4 - name: run apt update From 795800c36fd08a9dff6cdd8038349eaa4d4e3f01 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 21:00:45 +1000 Subject: [PATCH 62/72] noble_rolling needs libyaml-cpp-dev rosdep problem? --- .github/workflows/test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e8c6d94..5139935 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -87,6 +87,8 @@ jobs: PIP_BREAK_SYSTEM_PACKAGES: 1 - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + - name: Install libyaml-cpp-dev + run: sudo apt install libyaml-cpp-dev - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From 11e8e1a12288d42051decb86fe362cb34e125918 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 May 2024 21:10:15 +1000 Subject: [PATCH 63/72] jammy_rolling: workaround libgoogle-glog-dev libunwind-dev libceres-dev --- .github/workflows/test.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 5139935..1630b57 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -29,6 +29,10 @@ jobs: run: apt upgrade python3-rosdep -y - name: Install Cyclone DDS run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y + - name: workaround libgoogle-glog-dev dependency problem install libunwind-dev + run: sudo apt install libunwind-dev -y + - name: workaround libgoolg-glog-dev dependency problem install libceres-dev + run: sudo apt install libceres-dev -y - name: Temp Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28 2024 run: | sed -i 's@ros/rosdistro/master@ros/rosdistro/rolling/2024-02-28@' /etc/ros/rosdep/sources.list.d/20-default.list From 51b714ee3d6b5e15324efb1d6629581ae986ab67 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 30 May 2024 15:18:42 +1000 Subject: [PATCH 64/72] Abandon rolling on jammy --- .github/workflows/test.yml | 54 -------------------------------------- 1 file changed, 54 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 1630b57..5668c2f 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -7,60 +7,6 @@ on: - main jobs: - jammy_rolling: - runs-on: ubuntu-latest - env: - ROS_DISTRO: ${{ matrix.ros_distro }} - RMW_IMPLEMENTATION: rmw_cyclonedds_cpp - container: - image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest - strategy: - fail-fast: false - matrix: - ros_distro: [rolling] - steps: - - name: checkout source - uses: actions/checkout@v4 - - name: run apt update - run: apt update - - name: run apt list --upgradable - run: apt list --upgradeable - - name: run apt upgrade python3-rosdep -y - run: apt upgrade python3-rosdep -y - - name: Install Cyclone DDS - run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - name: workaround libgoogle-glog-dev dependency problem install libunwind-dev - run: sudo apt install libunwind-dev -y - - name: workaround libgoolg-glog-dev dependency problem install libceres-dev - run: sudo apt install libceres-dev -y - - name: Temp Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch use index prior to Feb 28 2024 - run: | - sed -i 's@ros/rosdistro/master@ros/rosdistro/rolling/2024-02-28@' /etc/ros/rosdep/sources.list.d/20-default.list - - name: Build and run tests - id: action-ros-ci - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: "opennav_coverage_demo" - target-ros2-distro: ${{ matrix.ros_distro }} - vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" - colcon-defaults: | - { - "test": { - "packages-select": [ - "opennav_coverage", - "opennav_coverage_bt", - "opennav_row_coverage", - "opennav_coverage_navigator", - "opennav_coverage_demo" - ] - } - } - env: - ROSDISTRO_INDEX_URL: https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml - - uses: actions/upload-artifact@v1 - with: - name: colcon-logs - path: ros_ws/log noble_rolling: runs-on: ubuntu-latest env: From f213e2f135f46709b7dadb5db708528c8d2c7cd0 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 30 May 2024 15:29:46 +1000 Subject: [PATCH 65/72] Strip away build on influx rolling workarounds --- .github/workflows/test.yml | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 5668c2f..d4e932c 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -11,7 +11,6 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: ${{ matrix.ros_distro }} - RMW_IMPLEMENTATION: rmw_cyclonedds_cpp container: image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest strategy: @@ -19,26 +18,8 @@ jobs: matrix: ros_distro: [rolling] steps: - - name: Use pre-release binaries from ros2-testing repository - v4.6 behaviortree_cpp - uses: ros-tooling/setup-ros@v0.7 - with: - use-ros2-testing: true - name: checkout source uses: actions/checkout@v4 - - name: run apt update - run: apt update - - name: run apt list --upgradable - run: apt list --upgradeable - - name: run apt upgrade python3-rosdep -y - run: apt upgrade python3-rosdep -y - - name: Unistall python nose - uses imp not supported python3.12 - run: pip uninstall nose -y - env: - PIP_BREAK_SYSTEM_PACKAGES: 1 - - name: Install Cyclone DDS - run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y - - name: Install libyaml-cpp-dev - run: sudo apt install libyaml-cpp-dev - name: Build and run tests id: action-ros-ci uses: ros-tooling/action-ros-ci@v0.3 From a4f275e6bc9cde1d0b69be93dc1147c833432547 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 30 May 2024 15:34:08 +1000 Subject: [PATCH 66/72] Comment out fields2cover from deps.repos, attempt to build against packages --- .github/deps.repos | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index 3352547..a1bd184 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -1,9 +1,9 @@ repositories: - Fields2Cover: - type: git - url: https://github.com/aosmw/Fields2Cover.git - version: feature/mw/ros2_build navigation2: type: git url: https://github.com/ros-planning/navigation2.git version: main + #Fields2Cover: + # type: git + # url: https://github.com/aosmw/Fields2Cover.git + # version: main From 02ee9c036bb91e533ddaaeffa2184c03fd09adff Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 3 Jun 2024 20:00:57 +1000 Subject: [PATCH 67/72] gh action: install nodejs for local build nektos/act nodejs in docker image is required to enable enable local debugging of github actions with https://github.com/nektos/act --- .github/workflows/test.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index d4e932c..147d732 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -18,6 +18,10 @@ jobs: matrix: ros_distro: [rolling] steps: + - name: Install node + run: | + sudo apt-get update + sudo apt-get -y install nodejs - name: checkout source uses: actions/checkout@v4 - name: Build and run tests From 8496db119f9a670d26285bc5c8b8c18c8d0e96bf Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 3 Jun 2024 20:14:27 +1000 Subject: [PATCH 68/72] Build fields2cover from source again This is to try to eliminate the possibility that we are fetching a bad package. --- .github/deps.repos | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index a1bd184..060e4ab 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -1,9 +1,9 @@ repositories: navigation2: type: git - url: https://github.com/ros-planning/navigation2.git + url: https://github.com/ros-navigation/navigation2.git + version: main + fields2cover: + type: git + url: https://github.com/Fields2Cover/Fields2Cover.git version: main - #Fields2Cover: - # type: git - # url: https://github.com/aosmw/Fields2Cover.git - # version: main From 50f4758f5d7b8b0550013d98ea74961cd6d800c8 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Mon, 3 Jun 2024 21:31:07 +1000 Subject: [PATCH 69/72] nav2_constrained_smoother has conflicted libunwind-dev, libgoogle-glog-dev for libceres-dev --- .github/workflows/test.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 147d732..563c256 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -18,10 +18,13 @@ jobs: matrix: ros_distro: [rolling] steps: - - name: Install node + - name: Install node, libunwind-dev, libgoogle-glog-dev run: | sudo apt-get update sudo apt-get -y install nodejs + sudo apt-get -y install libunwind-dev + sudo apt-get -y install libgoogle-glog-dev + sudo apt-get -y install libceres-dev - name: checkout source uses: actions/checkout@v4 - name: Build and run tests From 43748b278c7db17f3724bbbe02c8616e832060a2 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 30 May 2024 15:39:06 +1000 Subject: [PATCH 70/72] Add ortools_vendor to deps.repos --- .github/deps.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/deps.repos b/.github/deps.repos index 060e4ab..99f8e8f 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -7,3 +7,7 @@ repositories: type: git url: https://github.com/Fields2Cover/Fields2Cover.git version: main + ortools_vendor: + type: git + url: https://github.com/Fields2Cover/ortools_vendor.git + version: main From 6539b0ac3e66dd23a6be7c3bcc833447c2f0acd2 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 13 Jun 2024 10:59:24 +1000 Subject: [PATCH 71/72] Ignore vim .swp files --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 21696be..5e1b386 100644 --- a/.gitignore +++ b/.gitignore @@ -54,3 +54,6 @@ cmake-build-debug/ # doxygen docs doc/html/ + +#Vim +*.swp From 9d1ad837e2f6e3e57358e5a0433822acc359df4f Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 13 Jun 2024 11:08:04 +1000 Subject: [PATCH 72/72] deps.repos lock to tags for ortools_vendor and Fields2Cover NOTE: Fields2Cover still pointed at fork. --- .github/deps.repos | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index 99f8e8f..873c139 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -5,9 +5,9 @@ repositories: version: main fields2cover: type: git - url: https://github.com/Fields2Cover/Fields2Cover.git - version: main + url: https://github.com/aosmw/Fields2Cover.git + version: 9c3c352ba1dc71f3f48706c298b82cd4641e536e ortools_vendor: type: git url: https://github.com/Fields2Cover/ortools_vendor.git - version: main + version: c28f53e50164645fa8cee0a7a307061ac6f83aa7