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/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 diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 7e2a3e6..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 @@ -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 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/ 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. diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index 2e1ff4e..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 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/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_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/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/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..aa74893 100644 --- a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp +++ b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/utils/test_action_server.hpp" #include "opennav_coverage_bt/cancel_complete_coverage_path.hpp" @@ -125,7 +125,7 @@ TEST_F(CancelCoverageActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index 09ba0b8..afa81d8 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -20,7 +20,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/utils/test_action_server.hpp" #include "opennav_coverage_bt/compute_complete_coverage_path.hpp" @@ -123,7 +123,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + [[maybe_unused]] auto res = config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } @@ -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 index d315cfa..e417fdb 100644 --- a/opennav_coverage_demo/launch/coverage_demo_launch.py +++ b/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) diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py index 342fd84..f000119 100644 --- a/opennav_coverage_demo/launch/row_coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/row_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,55 +55,79 @@ 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', '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( - 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=['0', '0', '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(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 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 + + + + 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/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);