From 6fad68208e1df9038f9a44bc854679144977705c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 5 Nov 2024 15:16:02 +0000 Subject: [PATCH 01/25] [coverage_planning] make it build on rolling with F2C v1.2.1 --- opennav_coverage/CMakeLists.txt | 2 +- opennav_coverage/package.xml | 1 + 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 | 6 +++--- opennav_coverage_navigator/src/coverage_navigator.cpp | 4 ++-- opennav_row_coverage/CMakeLists.txt | 2 +- 11 files changed, 15 insertions(+), 14 deletions(-) diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 2e1ff4e..05bcc3a 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/opennav_coverage/CMakeLists.txt @@ -95,7 +95,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() - add_subdirectory(test) + # add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/opennav_coverage/package.xml b/opennav_coverage/package.xml index 93f41c7..628e420 100644 --- a/opennav_coverage/package.xml +++ b/opennav_coverage/package.xml @@ -9,6 +9,7 @@ ament_cmake + ament_index_cpp rclcpp rclcpp_action rclcpp_lifecycle 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 59b7fb6..6f988e2 100644 --- 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 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..00634e2 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" @@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + [[maybe_unused]] auto res = config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } diff --git a/opennav_coverage_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_; diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt index 2125c62..6a74564 100644 --- a/opennav_row_coverage/CMakeLists.txt +++ b/opennav_row_coverage/CMakeLists.txt @@ -80,7 +80,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() - add_subdirectory(test) + # add_subdirectory(test) endif() ament_export_include_directories(include) From 2360f65a66be6bbb1242a68e04d6d877f4ac087b Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Nov 2024 11:40:24 +0000 Subject: [PATCH 02/25] fix Werror=maybe-uninitialized --- opennav_coverage/src/coverage_server.cpp | 2 +- opennav_row_coverage/src/row_coverage_server.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opennav_coverage/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp index 0399e71..4e19c65 100644 --- 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_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp index 9533013..1699960 100644 --- a/opennav_row_coverage/src/row_coverage_server.cpp +++ b/opennav_row_coverage/src/row_coverage_server.cpp @@ -50,7 +50,7 @@ RowCoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/) node, "order_ids", rclcpp::ParameterValue(true)); get_parameter("order_ids", order_ids_); - 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); From 16af6a2002e8dadff21b518d8f75e73fed2c191f Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Nov 2024 12:50:14 +0100 Subject: [PATCH 03/25] restore tests and add missing ament_index_cpp to CMakeLists --- opennav_coverage/CMakeLists.txt | 4 +++- opennav_row_coverage/CMakeLists.txt | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 05bcc3a..13f5ae6 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) @@ -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 @@ -95,7 +97,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() - # add_subdirectory(test) + add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt index 6a74564..2125c62 100644 --- a/opennav_row_coverage/CMakeLists.txt +++ b/opennav_row_coverage/CMakeLists.txt @@ -80,7 +80,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() - # add_subdirectory(test) + add_subdirectory(test) endif() ament_export_include_directories(include) From 90577dbf301e200703a407cec85bb559fb79fe01 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Nov 2024 15:26:11 +0100 Subject: [PATCH 04/25] update demo launcher for Gazebo Ignition --- .../launch/coverage_demo_launch.py | 67 ++++++++++++------- opennav_coverage_demo/package.xml | 2 + 2 files changed, 44 insertions(+), 25 deletions(-) diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py index d315cfa..8ebef03 100644 --- a/opennav_coverage_demo/launch/coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/coverage_demo_launch.py @@ -13,11 +13,12 @@ # 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 +27,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') - - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + 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(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: robot_description = infp.read() @@ -53,15 +53,29 @@ def generate_launch_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( @@ -90,10 +104,13 @@ def generate_launch_description(): 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) diff --git a/opennav_coverage_demo/package.xml b/opennav_coverage_demo/package.xml index e9d09a1..c6cf189 100644 --- a/opennav_coverage_demo/package.xml +++ b/opennav_coverage_demo/package.xml @@ -17,6 +17,8 @@ opennav_row_coverage opennav_coverage_navigator + nav2_minimal_tb3_sim + ament_lint_common ament_lint_auto From a1c843a5b6cce51bf768fa86d86b50008ea45ada Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Nov 2024 15:26:58 +0100 Subject: [PATCH 05/25] explicitly add models to avoid errors --- opennav_coverage_demo/world/blank.world | 49 ++++++++++++++++++++----- 1 file changed, 40 insertions(+), 9 deletions(-) diff --git a/opennav_coverage_demo/world/blank.world b/opennav_coverage_demo/world/blank.world index f768d07..38f306b 100644 --- a/opennav_coverage_demo/world/blank.world +++ b/opennav_coverage_demo/world/blank.world @@ -3,14 +3,45 @@ - - model://ground_plane - - - - - model://sun - - + + 0 + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + 10 + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + From d4b634a4da3b7323e8ab841e19045abc7ba70922 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 21 Nov 2024 15:31:16 +0100 Subject: [PATCH 06/25] 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. --- opennav_coverage_demo/launch/coverage_demo_launch.py | 4 ++-- opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py index 8ebef03..fa35dd5 100644 --- a/opennav_coverage_demo/launch/coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/coverage_demo_launch.py @@ -70,8 +70,8 @@ def generate_launch_description(): launch_arguments={'namespace': '', 'robot_name': 'turtlebot3_waffle', 'robot_sdf': robot_sdf, - 'x_pose': str(5.0), - 'y_pose': str(5.0), + 'x_pose': str(0.0), + 'y_pose': str(0.0), 'z_pose': str(0.1), 'roll': str(0.0), 'pitch': str(0.0), diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py index 5435451..f2f0d5e 100644 --- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py +++ b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py @@ -146,7 +146,7 @@ def main(): navigator.startup() # Some example field - field = [[5.0, 5.0], [5.0, 15.0], [15.0, 15.0], [10.0, 5.0], [5.0, 5.0]] + field = [[0.0, 0.0], [0.0, 15.0], [15.0, 15.0], [10.0, 0.0], [0.0, 0.0]] navigator.navigateCoverage(field) i = 0 From c1b1423b792e1755d612b3ed579f820a885b087a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 22 Nov 2024 08:57:07 +0000 Subject: [PATCH 07/25] Add .gitignore Copied from ros-navigation/navigation2 --- .gitignore | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..21696be --- /dev/null +++ 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/ From d54a1d60a374a9e7ac194f597cedd902613d7643 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 25 Nov 2024 16:17:13 +0100 Subject: [PATCH 08/25] 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 --- opennav_coverage_demo/launch/coverage_demo_launch.py | 6 +++--- .../opennav_coverage_demo/demo_coverage.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py index fa35dd5..2c3f783 100644 --- a/opennav_coverage_demo/launch/coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/coverage_demo_launch.py @@ -70,8 +70,8 @@ def generate_launch_description(): launch_arguments={'namespace': '', 'robot_name': 'turtlebot3_waffle', 'robot_sdf': robot_sdf, - 'x_pose': str(0.0), - 'y_pose': str(0.0), + 'x_pose': str(5.0), + 'y_pose': str(5.0), 'z_pose': str(0.1), 'roll': str(0.0), 'pitch': str(0.0), @@ -94,7 +94,7 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']) + arguments=['5.0', '5.0', '0', '0', '0', '0', 'map', 'odom']) # start the demo task demo_cmd = Node( diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py index f2f0d5e..5435451 100644 --- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py +++ b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py @@ -146,7 +146,7 @@ def main(): navigator.startup() # Some example field - field = [[0.0, 0.0], [0.0, 15.0], [15.0, 15.0], [10.0, 0.0], [0.0, 0.0]] + field = [[5.0, 5.0], [5.0, 15.0], [15.0, 15.0], [10.0, 5.0], [5.0, 5.0]] navigator.navigateCoverage(field) i = 0 From ad7110ec91abd4b204339fa429834d1267e01b20 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 25 Nov 2024 16:25:54 +0100 Subject: [PATCH 09/25] Update row_coverage_demo_launch.py to Gazebo Ignition --- .../launch/row_coverage_demo_launch.py | 69 ++++++++++++------- 1 file changed, 43 insertions(+), 26 deletions(-) diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py index 342fd84..1fb6f05 100644 --- a/opennav_coverage_demo/launch/row_coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/row_coverage_demo_launch.py @@ -13,11 +13,12 @@ # 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 +27,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') - - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + 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(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: robot_description = infp.read() @@ -53,15 +53,29 @@ def generate_launch_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', '6.23', '-y', '15.0', '-z', '0.10', - '-R', '0.0', '-P', '0.0', '-Y', '-1.5708']) + 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(6.23), + 'y_pose': str(15.0), + 'z_pose': str(0.1), + 'roll': str(0.0), + 'pitch': str(0.0), + 'yaw': str(-1.5708)}.items()) # start the visualization rviz_cmd = IncludeLaunchDescription( @@ -80,7 +94,7 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']) + arguments=['6.23', '15', '0', '0', '0', '0', 'map', 'odom']) fake_gps_cmd = Node( package='tf2_ros', executable='static_transform_publisher', @@ -95,10 +109,13 @@ def generate_launch_description(): 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) From 247706ceb00d72eba2de5bf71021a4c31edfbb9d Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 25 Nov 2024 16:56:44 +0100 Subject: [PATCH 10/25] Note on F2C version --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index 7dd8435..ca0b40a 100644 --- 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. From d8e7df45024a4fbe2054b308d551e575998e269c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 25 Nov 2024 16:58:54 +0100 Subject: [PATCH 11/25] Update test workflow --- .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..ec4680c 100644 --- a/.github/workflows/test.yml +++ b/.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 From ad496a19a80f05271fee509cef3ebe8928e0d4e3 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 25 Nov 2024 17:02:18 +0100 Subject: [PATCH 12/25] lint and format --- .../launch/coverage_demo_launch.py | 79 ++++++++++------- .../launch/row_coverage_demo_launch.py | 88 +++++++++++-------- 2 files changed, 95 insertions(+), 72 deletions(-) diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py index 2c3f783..e417fdb 100644 --- a/opennav_coverage_demo/launch/coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/coverage_demo_launch.py @@ -17,7 +17,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler +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 @@ -27,18 +32,18 @@ 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") + 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') - robot_sdf = os.path.join(sim_dir, "urdf", "gz_waffle.sdf.xacro") + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') # start the simulation - world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf") - world_sdf_xacro = ExecuteProcess(cmd=["xacro", "-o", world_sdf, "headless:=false", world]) + 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", + cmd=['gz', 'sim', '-r', '-s', world_sdf], + output='screen', ) urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') @@ -50,58 +55,64 @@ 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}], + ) remove_temp_sdf_file = RegisterEventHandler( - event_handler=OnShutdown(on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))]) + 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") + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py') ), - launch_arguments={"gz_args": ["-v4 -g "]}.items(), + 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()) + 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=['5.0', '5.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(world_sdf_xacro) diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py index 1fb6f05..f000119 100644 --- a/opennav_coverage_demo/launch/row_coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/row_coverage_demo_launch.py @@ -17,7 +17,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler +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 @@ -27,18 +32,18 @@ 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") + 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') - robot_sdf = os.path.join(sim_dir, "urdf", "gz_waffle.sdf.xacro") + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') # start the simulation - world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf") - world_sdf_xacro = ExecuteProcess(cmd=["xacro", "-o", world_sdf, "headless:=false", world]) + 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", + cmd=['gz', 'sim', '-r', '-s', world_sdf], + output='screen', ) urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') @@ -50,63 +55,70 @@ 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}], + ) remove_temp_sdf_file = RegisterEventHandler( - event_handler=OnShutdown(on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))]) + 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") + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py') ), - launch_arguments={"gz_args": ["-v4 -g "]}.items(), + 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(6.23), - 'y_pose': str(15.0), - 'z_pose': str(0.1), - 'roll': str(0.0), - 'pitch': str(0.0), - 'yaw': str(-1.5708)}.items()) + 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(6.23), + 'y_pose': str(15.0), + 'z_pose': str(0.1), + 'roll': str(0.0), + 'pitch': str(0.0), + 'yaw': str(-1.5708), + }.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, 'row_bringup_launch.py')), - launch_arguments={'params_file': param_file_path}.items()) + PythonLaunchDescriptionSource(os.path.join(coverage_demo_dir, 'row_bringup_launch.py')), + launch_arguments={'params_file': param_file_path}.items(), + ) # Demo GPS->map->odom transform, no localization. For visualization & controller transform fake_localization_cmd = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['6.23', '15', '0', '0', '0', '0', 'map', 'odom']) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['6.23', '15', '0', '0', '0', '0', 'map', 'odom'], + ) fake_gps_cmd = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'EPSG:4258', 'map']) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'EPSG:4258', 'map'], + ) # start the demo task demo_cmd = Node( package='opennav_coverage_demo', executable='demo_row_coverage', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(world_sdf_xacro) From 6f749e015ed41941412bde7d91951fb43f7dbffa Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 26 Nov 2024 11:50:31 +0100 Subject: [PATCH 13/25] add BTCPP_format to BT xmls --- .../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..aa74893 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 00634e2..f225c75 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 8f1946e7a3c7fdb256464e332d987b33bfc9591e Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 26 Nov 2024 11:51:05 +0100 Subject: [PATCH 14/25] CI: change F2C branch and update to Jazzy & Noble --- .github/deps.repos | 2 +- .github/workflows/test.yml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index ef88206..1fe01fa 100644 --- 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/test.yml b/.github/workflows/test.yml index ec4680c..e52fdb9 100644 --- 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 From cb659870b7755b7150bc169d5d2018e50ccaa507 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 27 Nov 2024 10:42:12 +0100 Subject: [PATCH 15/25] try F2C v1.2.1 --- .github/deps.repos | 2 +- opennav_coverage/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index 1fe01fa..d3309e6 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -2,4 +2,4 @@ repositories: Fields2Cover: type: git url: https://github.com/Fields2Cover/Fields2Cover.git - version: v1.2.1-devel + version: v1.2.1 diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 13f5ae6..599fa4f 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/opennav_coverage/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(Fields2Cover REQUIRED) # potentially replace with nav2_common, nav2_package() set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) +# add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) include_directories( include From 8164091dbb7292deeae9998a1ddf8a7f4ec8b10c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 27 Nov 2024 11:22:55 +0100 Subject: [PATCH 16/25] [tmp] ignore F2C compile errors --- .github/workflows/test.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e52fdb9..2f5c223 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -28,6 +28,14 @@ jobs: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" + colcon-defaults: | + { + "build": { + "cmake-args": [ + "-DCMAKE_CXX_FLAGS=-Wno-error=pessimizing-move -Wno-error=deprecated-declarations" + ] + } + } - uses: actions/upload-artifact@v4 with: name: colcon-logs From 3fc2c298305a55a9f6387e556fde00db9586f36c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 27 Nov 2024 11:57:34 +0100 Subject: [PATCH 17/25] Update lint step to jazzy --- .github/workflows/lint.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index c604ac9..b20206a 100644 --- 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 From 3a9e9d12b1cd503ce60d9f48c9e84b1b13f8780f Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 27 Nov 2024 14:31:05 +0000 Subject: [PATCH 18/25] test with cartesian polygon --- opennav_coverage/test/test_server.cpp | 2 +- opennav_coverage/test/tester.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opennav_coverage/test/test_server.cpp b/opennav_coverage/test/test_server.cpp index edd033b..f1cc3bb 100644 --- a/opennav_coverage/test/test_server.cpp +++ b/opennav_coverage/test/test_server.cpp @@ -98,7 +98,7 @@ TEST(ServerTest, testServerTransactions) auto goal_msg = opennav_coverage_msgs::action::ComputeCoveragePath::Goal(); goal_msg.use_gml_file = true; // Use file goal_msg.gml_field = - ament_index_cpp::get_package_share_directory("opennav_coverage") + "/test_field.xml"; + ament_index_cpp::get_package_share_directory("opennav_coverage") + "/cartesian_test_field.xml"; auto future_goal_handle = action_client->async_send_goal(goal_msg); EXPECT_EQ( diff --git a/opennav_coverage/test/tester.py b/opennav_coverage/test/tester.py index ecb541f..7b8716c 100644 --- a/opennav_coverage/test/tester.py +++ b/opennav_coverage/test/tester.py @@ -125,7 +125,7 @@ def main(): goal.generate_path = True goal.use_gml_file = True - goal.gml_field = get_package_share_directory('opennav_coverage') + '/test_field.xml' + goal.gml_field = get_package_share_directory('opennav_coverage') + '/cartesian_test_field.xml' goal.gml_field_id = 0 # Cartesian coordinates corresponding to a similar XML as test_field.xml From 964bbedcdadf4a4c946886696402a3d26ff8d180 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 28 Nov 2024 08:28:34 +0000 Subject: [PATCH 19/25] Revert "try F2C v1.2.1" This reverts commit cb659870b7755b7150bc169d5d2018e50ccaa507. --- .github/deps.repos | 2 +- opennav_coverage/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/deps.repos b/.github/deps.repos index d3309e6..1fe01fa 100644 --- a/.github/deps.repos +++ b/.github/deps.repos @@ -2,4 +2,4 @@ repositories: Fields2Cover: type: git url: https://github.com/Fields2Cover/Fields2Cover.git - version: v1.2.1 + version: v1.2.1-devel diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 599fa4f..13f5ae6 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/opennav_coverage/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(Fields2Cover REQUIRED) # potentially replace with nav2_common, nav2_package() set(CMAKE_CXX_STANDARD 17) -# add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) +add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) include_directories( include From 30658a2049dc4dd169df1335c3d887205cffdab6 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 28 Nov 2024 08:28:58 +0000 Subject: [PATCH 20/25] Revert "[tmp] ignore F2C compile errors" This reverts commit 8164091dbb7292deeae9998a1ddf8a7f4ec8b10c. --- .github/workflows/test.yml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2f5c223..e52fdb9 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -28,14 +28,6 @@ jobs: import-token: ${{ secrets.GITHUB_TOKEN }} target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" - colcon-defaults: | - { - "build": { - "cmake-args": [ - "-DCMAKE_CXX_FLAGS=-Wno-error=pessimizing-move -Wno-error=deprecated-declarations" - ] - } - } - uses: actions/upload-artifact@v4 with: name: colcon-logs From 6a6194078fc636b8dd4446393bfb5da055483cde Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 28 Nov 2024 11:02:31 +0000 Subject: [PATCH 21/25] [tmp] rm failing test in opennav_coverage_bt --- opennav_coverage_bt/test/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/opennav_coverage_bt/test/CMakeLists.txt b/opennav_coverage_bt/test/CMakeLists.txt index 5f7e08d..7021b9c 100644 --- a/opennav_coverage_bt/test/CMakeLists.txt +++ b/opennav_coverage_bt/test/CMakeLists.txt @@ -4,6 +4,6 @@ target_link_libraries(test_cancel_complete_coverage opennav_cancel_complete_cove ament_target_dependencies(test_cancel_complete_coverage ${dependencies}) # Make command test -ament_add_gtest(test_compute_coverage_path test_compute_coverage_path.cpp) -target_link_libraries(test_compute_coverage_path opennav_compute_complete_coverage_action_bt_node) -ament_target_dependencies(test_compute_coverage_path ${dependencies}) +# ament_add_gtest(test_compute_coverage_path test_compute_coverage_path.cpp) +# target_link_libraries(test_compute_coverage_path opennav_compute_complete_coverage_action_bt_node) +# ament_target_dependencies(test_compute_coverage_path ${dependencies}) From feb6411f0234d4fb8068d7a10d123efdf4640292 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 2 Dec 2024 09:43:31 +0000 Subject: [PATCH 22/25] switch test to cartesian_frame --- opennav_coverage/test/test_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opennav_coverage/test/test_server.cpp b/opennav_coverage/test/test_server.cpp index f1cc3bb..08e1d02 100644 --- a/opennav_coverage/test/test_server.cpp +++ b/opennav_coverage/test/test_server.cpp @@ -41,7 +41,7 @@ class ServerShim : public opennav_coverage::CoverageServer void configure(const rclcpp_lifecycle::State & state) { this->on_configure(state); - cartesian_frame_ = false; // Test files in GPS + cartesian_frame_ = true; // Test files in GPS } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} From 2f55bc3c289137e56ed4063bc429887117aaf800 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 2 Dec 2024 09:51:46 +0000 Subject: [PATCH 23/25] restore non-cartesian field test --- opennav_coverage/test/test_server.cpp | 4 ++-- opennav_coverage/test/tester.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/opennav_coverage/test/test_server.cpp b/opennav_coverage/test/test_server.cpp index 08e1d02..edd033b 100644 --- a/opennav_coverage/test/test_server.cpp +++ b/opennav_coverage/test/test_server.cpp @@ -41,7 +41,7 @@ class ServerShim : public opennav_coverage::CoverageServer void configure(const rclcpp_lifecycle::State & state) { this->on_configure(state); - cartesian_frame_ = true; // Test files in GPS + cartesian_frame_ = false; // Test files in GPS } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} @@ -98,7 +98,7 @@ TEST(ServerTest, testServerTransactions) auto goal_msg = opennav_coverage_msgs::action::ComputeCoveragePath::Goal(); goal_msg.use_gml_file = true; // Use file goal_msg.gml_field = - ament_index_cpp::get_package_share_directory("opennav_coverage") + "/cartesian_test_field.xml"; + ament_index_cpp::get_package_share_directory("opennav_coverage") + "/test_field.xml"; auto future_goal_handle = action_client->async_send_goal(goal_msg); EXPECT_EQ( diff --git a/opennav_coverage/test/tester.py b/opennav_coverage/test/tester.py index 7b8716c..ecb541f 100644 --- a/opennav_coverage/test/tester.py +++ b/opennav_coverage/test/tester.py @@ -125,7 +125,7 @@ def main(): goal.generate_path = True goal.use_gml_file = True - goal.gml_field = get_package_share_directory('opennav_coverage') + '/cartesian_test_field.xml' + goal.gml_field = get_package_share_directory('opennav_coverage') + '/test_field.xml' goal.gml_field_id = 0 # Cartesian coordinates corresponding to a similar XML as test_field.xml From 5ff83b300c739d27cb838df164ca8c308e31eb96 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 2 Dec 2024 10:13:31 +0000 Subject: [PATCH 24/25] Revert "[tmp] rm failing test in opennav_coverage_bt" This reverts commit 6a6194078fc636b8dd4446393bfb5da055483cde. --- opennav_coverage_bt/test/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/opennav_coverage_bt/test/CMakeLists.txt b/opennav_coverage_bt/test/CMakeLists.txt index 7021b9c..5f7e08d 100644 --- a/opennav_coverage_bt/test/CMakeLists.txt +++ b/opennav_coverage_bt/test/CMakeLists.txt @@ -4,6 +4,6 @@ target_link_libraries(test_cancel_complete_coverage opennav_cancel_complete_cove ament_target_dependencies(test_cancel_complete_coverage ${dependencies}) # Make command test -# ament_add_gtest(test_compute_coverage_path test_compute_coverage_path.cpp) -# target_link_libraries(test_compute_coverage_path opennav_compute_complete_coverage_action_bt_node) -# ament_target_dependencies(test_compute_coverage_path ${dependencies}) +ament_add_gtest(test_compute_coverage_path test_compute_coverage_path.cpp) +target_link_libraries(test_compute_coverage_path opennav_compute_complete_coverage_action_bt_node) +ament_target_dependencies(test_compute_coverage_path ${dependencies}) From 84d85c88d5aff8a0787799930be25a7bbd8617f0 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 2 Dec 2024 10:42:22 +0000 Subject: [PATCH 25/25] 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. --- 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 f225c75..afa81d8 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -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;