Update to Jazzy/Rolling with F2C v1.2.1 (#82)

adivardi · web-flow · commit 194250501889 · 2024-12-02T14:45:02.000-08:00
* [coverage_planning] make it build on rolling with F2C v1.2.1 * fix Werror=maybe-uninitialized * restore tests and add missing ament_index_cpp to CMakeLists * update demo launcher for Gazebo Ignition * explicitly add models to avoid errors * move robot pose and polygon to (0,0) In GZ Ignition, tb3 publishes its ground truth pose as odometry with respect to its start position. Even when spawning the model at (5,5), the map->base_link transform is 0 because the robot did not drive. This causes navigation to fail as the robot pose is too far from the start of the path. * Add .gitignore Copied from ros-navigation/navigation2 * Restore robot pose & polygon to (5,5) and add (5,5) to map->odom tf as it is the transform from world/map frame to the robot's initial pose * Update row_coverage_demo_launch.py to Gazebo Ignition * Note on F2C version * Update test workflow * lint and format * add BTCPP_format to BT xmls * CI: change F2C branch and update to Jazzy & Noble * try F2C v1.2.1 * [tmp] ignore F2C compile errors * Update lint step to jazzy * test with cartesian polygon * Revert "try F2C v1.2.1" This reverts commit cb65987. * Revert "[tmp] ignore F2C compile errors" This reverts commit 8164091. * [tmp] rm failing test in opennav_coverage_bt * switch test to cartesian_frame * restore non-cartesian field test * Revert "[tmp] rm failing test in opennav_coverage_bt" This reverts commit 6a61940. * restore rclcpp:shutdown() Without the shutdown: The tests are passing in `RUN_ALL_TESTS()`, the test itself is failing with timeout. Most probably because `server_thread.join()` hangs forever.
diff --git a/.github/deps.repos b/.github/deps.repos
@@ -2,4 +2,4 @@ repositories:
   Fields2Cover:
     type: git
     url: https://github.com/Fields2Cover/Fields2Cover.git
-    version: main
+    version: v1.2.1-devel
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
@@ -7,7 +7,7 @@ jobs:
     name: ament_${{ matrix.linter }}
     runs-on: ubuntu-latest
     container:
-      image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest
+      image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest
     strategy:
       fail-fast: false
       matrix:
@@ -17,7 +17,7 @@ jobs:
     - uses: ros-tooling/action-ros-lint@v0.1
       with:
         linter: ${{ matrix.linter }}
-        distribution: iron
+        distribution: jazzy
         package-name: |
             opennav_coverage
             opennav_coverage_bt
diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
@@ -12,11 +12,11 @@ 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-noble-latest
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [iron]
+          ros_distro: [jazzy]
     steps:
     - uses: actions/checkout@v2
     - name: Install Cyclone DDS
@@ -28,7 +28,7 @@ jobs:
         import-token: ${{ secrets.GITHUB_TOKEN }}
         target-ros2-distro: ${{ matrix.ros_distro }}
         vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos"
-    - uses: actions/upload-artifact@v1
+    - uses: actions/upload-artifact@v4
       with:
         name: colcon-logs
         path: ros_ws/log
diff --git a/.gitignore b/.gitignore
@@ -0,0 +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/README.md b/README.md
@@ -34,6 +34,15 @@ PS: Click on either image to see the demo videos! :-)
 
 https://github.com/user-attachments/assets/e44d8f10-c5b0-4345-81ad-25f3bcd06030
 
+### Fields2Cover Installation
+
+opennav_coverage works with Field2Cover v1.2.1. The newest version v2.0.0 is not supported at the moment.
+
+To install Fields2Cover, clone it into a colcon workspace and build with `colcon build`.
+
+- Humble & Iron: Use tag `v1.2.1`
+- Jazzy & rolling: Use branch `v1.2.1-devel`
+
 ## Interfaces
 
 The two main interfaces are `NavigateCompleteCoverage` and `ComputeCoveragePath`. The first is the action definition to request the BT Navigator's `CoverageNavigator` plugin to navigate usign a Complete Coverage task input. The latter is an analog to the `PlannerServer`'s action definition for computing Complete Coverage paths using the `opennav_coverage` action server. See `opennav_coverage_msgs` for complete details.
diff --git 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)
@@ -29,6 +30,7 @@ set(executable_name opennav_coverage)
 set(library_name ${executable_name}_core)
 
 set(dependencies
+  ament_index_cpp
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
diff --git a/opennav_coverage/package.xml b/opennav_coverage/package.xml
@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
diff --git a/opennav_coverage/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp
@@ -46,7 +46,7 @@ CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     node, "coordinates_in_cartesian_frame", rclcpp::ParameterValue(true));
   get_parameter("coordinates_in_cartesian_frame", cartesian_frame_);
 
-  double action_server_result_timeout;
+  double action_server_result_timeout = 10.0;
   nav2_util::declare_parameter_if_not_declared(
     node, "action_server_result_timeout", rclcpp::ParameterValue(10.0));
   get_parameter("action_server_result_timeout", action_server_result_timeout);
diff --git a/opennav_coverage_bt/CMakeLists.txt b/opennav_coverage_bt/CMakeLists.txt
@@ -12,7 +12,7 @@ find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -33,7 +33,7 @@ set(dependencies
   nav_msgs
   geometry_msgs
   opennav_coverage_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
 )
 
 add_library(opennav_compute_complete_coverage_action_bt_node SHARED src/compute_complete_coverage_path.cpp)
diff --git a/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml b/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml
@@ -11,7 +11,7 @@
   This BT shows field file usage with the row coverage server
 -->
 
-<root main_tree_to_execute="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <RateController hz="0.0000001"> <!-- once, for demo -->
       <Sequence name="NavigateWithoutReplanning">
diff --git 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 <charconv>
-#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
@@ -18,7 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>opennav_coverage_msgs</depend>
-  <depend>behaviortree_cpp_v3</depend>
+  <depend>behaviortree_cpp</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git 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
@@ -99,7 +99,7 @@ void ComputeCoveragePathAction::halt()
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =
diff --git a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp
@@ -17,7 +17,7 @@
 #include <set>
 #include <string>
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/cancel_complete_coverage_path.hpp"
@@ -125,7 +125,7 @@ TEST_F(CancelCoverageActionTestFixture, test_ports)
 {
   std::string xml_txt =
     R"(
-      <root main_tree_to_execute = "MainTree" >
+      <root BTCPP_format="4" main_tree_to_execute="MainTree">
         <BehaviorTree ID="MainTree">
              <CancelCoverage name="CoverageCancel"/>
         </BehaviorTree>
diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp
@@ -20,7 +20,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/compute_complete_coverage_path.hpp"
@@ -123,7 +123,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
   // create tree
   std::string xml_txt =
     R"(
-      <root main_tree_to_execute = "MainTree" >
+      <root BTCPP_format="4" main_tree_to_execute="MainTree">
         <BehaviorTree ID="MainTree">
             <ComputeCoveragePath nav_path="{path}"/>
         </BehaviorTree>
@@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
 
   // check if returned path is correct
   nav_msgs::msg::Path path;
-  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  [[maybe_unused]] auto res = config_->blackboard->get<nav_msgs::msg::Path>("path", path);
   EXPECT_EQ(path.poses.size(), 2u);
   EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
   EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
 
   // halt node so another goal can be sent
-  tree_->rootNode()->halt();
+  tree_->haltTree();
   EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
 }
 
@@ -169,7 +169,7 @@ int main(int argc, char ** argv)
   int all_successful = RUN_ALL_TESTS();
 
   // shutdown ROS
-  // rclcpp::shutdown();
+  rclcpp::shutdown();
   server_thread.join();
 
   return all_successful;
diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py
diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py
diff --git a/opennav_coverage_demo/package.xml b/opennav_coverage_demo/package.xml
diff --git a/opennav_coverage_demo/world/blank.world b/opennav_coverage_demo/world/blank.world
diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp
diff --git a/opennav_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp

Original file line number	Diff line number	Diff line change
`@@ -16,7 +16,7 @@`
`16`	`16`	`#define OPENNAV_COVERAGE_BT__UTILS_HPP_`
`17`	`17`
`18`	`18`	`#include <charconv>`
`19`		`-#include "behaviortree_cpp_v3/behavior_tree.h"`
	`19`	`+#include "behaviortree_cpp/behavior_tree.h"`
`20`	`20`
`21`	`21`	`namespace BT`
`22`	`22`	`{`
Original file line number	Diff line number	Diff line change
`@@ -31,7 +31,7 @@ CoverageCancel::CoverageCancel(`
`31`	`31`
`32`	`32`	`} // namespace opennav_coverage_bt`
`33`	`33`
`34`		`-#include "behaviortree_cpp_v3/bt_factory.h"`
	`34`	`+#include "behaviortree_cpp/bt_factory.h"`
`35`	`35`	`BT_REGISTER_NODES(factory)`
`36`	`36`	`{`
`37`	`37`	`BT::NodeBuilder builder =`
Original file line number	Diff line number	Diff line change
`@@ -99,7 +99,7 @@ void ComputeCoveragePathAction::halt()`
`99`	`99`
`100`	`100`	`} // namespace opennav_coverage_bt`
`101`	`101`
`102`		`-#include "behaviortree_cpp_v3/bt_factory.h"`
	`102`	`+#include "behaviortree_cpp/bt_factory.h"`
`103`	`103`	`BT_REGISTER_NODES(factory)`
`104`	`104`	`{`
`105`	`105`	`BT::NodeBuilder builder =`