Update to Jazzy/Rolling with F2C v1.2.1

.github/workflows/test.yml

@@ -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

.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/

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.

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

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>

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);

opennav_coverage_bt/CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -33,7 +33,7 @@ set(dependencies
   nav_msgs
   geometry_msgs
   opennav_coverage_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
 )
 
 add_library(opennav_compute_complete_coverage_action_bt_node SHARED src/compute_complete_coverage_path.cpp)

opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp

@@ -16,7 +16,7 @@
 #define OPENNAV_COVERAGE_BT__UTILS_HPP_
 
 #include <charconv>
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace BT
 {

opennav_coverage_bt/package.xml

@@ -18,7 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>opennav_coverage_msgs</depend>
-  <depend>behaviortree_cpp_v3</depend>
+  <depend>behaviortree_cpp</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

opennav_coverage_bt/src/cancel_complete_coverage_path.cpp

@@ -31,7 +31,7 @@ CoverageCancel::CoverageCancel(
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =

opennav_coverage_bt/src/compute_complete_coverage_path.cpp

@@ -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 =

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"

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"
@@ -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);
 }
 

opennav_coverage_demo/launch/coverage_demo_launch.py

@@ -13,11 +13,17 @@
 # limitations under the License.
 
 import os
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.actions import (
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from launch.event_handlers import OnShutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
@@ -26,22 +32,21 @@ def generate_launch_description():
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     coverage_demo_dir = get_package_share_directory('opennav_coverage_demo')
     rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
 
     world = os.path.join(coverage_demo_dir, 'blank.world')
     param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml')
-    sdf = os.path.join(nav2_bringup_dir, 'worlds', 'waffle.model')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro')
 
     # start the simulation
-    start_gazebo_server_cmd = ExecuteProcess(
-        cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
-             '-s', 'libgazebo_ros_factory.so', world],
-        cwd=[coverage_demo_dir], output='screen')
-
-    # start_gazebo_client_cmd = ExecuteProcess(
-    #     cmd=['gzclient'],
-    #     cwd=[coverage_demo_dir], output='screen')
+    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
+    world_sdf_xacro = ExecuteProcess(cmd=['xacro', '-o', world_sdf, 'headless:=false', world])
+    gazebo_server = ExecuteProcess(
+        cmd=['gz', 'sim', '-r', '-s', world_sdf],
+        output='screen',
+    )
 
-    urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
     with open(urdf, 'r') as infp:
         robot_description = infp.read()
 
@@ -50,50 +55,73 @@ def generate_launch_description():
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
-        parameters=[{'use_sim_time': True,
-                     'robot_description': robot_description}])
+        parameters=[{'use_sim_time': True, 'robot_description': robot_description}],
+    )
 
-    start_gazebo_spawner_cmd = Node(
-        package='gazebo_ros',
-        executable='spawn_entity.py',
-        output='screen',
-        arguments=[
-            '-entity', 'tb3',
-            '-file', sdf,
-            '-x', '5.0', '-y', '5.0', '-z', '0.10',
-            '-R', '0.0', '-P', '0.0', '-Y', '0.0'])
+    remove_temp_sdf_file = RegisterEventHandler(
+        event_handler=OnShutdown(
+            on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))]
+        )
+    )
+
+    gazebo_client = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
+        ),
+        launch_arguments={'gz_args': ['-v4 -g ']}.items(),
+    )
+
+    start_gazebo_spawner_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')),
+        launch_arguments={
+            'namespace': '',
+            'robot_name': 'turtlebot3_waffle',
+            'robot_sdf': robot_sdf,
+            'x_pose': str(5.0),
+            'y_pose': str(5.0),
+            'z_pose': str(0.1),
+            'roll': str(0.0),
+            'pitch': str(0.0),
+            'yaw': str(0.0),
+        }.items(),
+    )
 
     # start the visualization
     rviz_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
-        launch_arguments={'namespace': '', 'rviz_config': rviz_config_file}.items())
+        PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
+        launch_arguments={'namespace': '', 'rviz_config': rviz_config_file}.items(),
+    )
 
     # start navigation
     bringup_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(coverage_demo_dir, 'bringup_launch.py')),
-        launch_arguments={'params_file': param_file_path}.items())
+        PythonLaunchDescriptionSource(os.path.join(coverage_demo_dir, 'bringup_launch.py')),
+        launch_arguments={'params_file': param_file_path}.items(),
+    )
 
     # world->odom transform, no localization. For visualization & controller transform
     fake_localization_cmd = Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            output='screen',
-            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'])
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['5.0', '5.0', '0', '0', '0', '0', 'map', 'odom'],
+    )
 
     # start the demo task
     demo_cmd = Node(
         package='opennav_coverage_demo',
         executable='demo_coverage',
         emulate_tty=True,
-        output='screen')
+        output='screen',
+    )
 
     ld = LaunchDescription()
-    ld.add_action(start_gazebo_server_cmd)
-    # ld.add_action(start_gazebo_client_cmd)
-    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(world_sdf_xacro)
+    ld.add_action(remove_temp_sdf_file)
+    ld.add_action(gazebo_server)
+    ld.add_action(gazebo_client)
     ld.add_action(start_gazebo_spawner_cmd)
+
+    ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(rviz_cmd)
     ld.add_action(bringup_cmd)
     ld.add_action(fake_localization_cmd)