├── .github ├── deps.repos └── workflows │ ├── lint.yml │ └── test.yml ├── .gitignore ├── LICENSE ├── README.md ├── opennav_coverage ├── CMakeLists.txt ├── README.md ├── include │ └── opennav_coverage │ │ ├── coverage_server.hpp │ │ ├── headland_generator.hpp │ │ ├── path_generator.hpp │ │ ├── robot_params.hpp │ │ ├── route_generator.hpp │ │ ├── swath_generator.hpp │ │ ├── types.hpp │ │ ├── utils.hpp │ │ └── visualizer.hpp ├── package.xml ├── src │ ├── coverage_server.cpp │ ├── headland_generator.cpp │ ├── main.cpp │ ├── path_generator.cpp │ ├── route_generator.cpp │ ├── swath_generator.cpp │ └── visualizer.cpp └── test │ ├── CMakeLists.txt │ ├── cartesian_test_field.xml │ ├── irregular_test_field.xml │ ├── test_field.xml │ ├── test_headland.cpp │ ├── test_path.cpp │ ├── test_robot.cpp │ ├── test_route.cpp │ ├── test_server.cpp │ ├── test_swath.cpp │ ├── test_utils.cpp │ ├── test_viz.cpp │ └── tester.py ├── opennav_coverage_bt ├── CMakeLists.txt ├── README.md ├── behavior_trees │ ├── navigate_w_basic_complete_coverage.xml │ ├── navigate_w_basic_complete_coverage_nav_to_start.xml │ └── navigate_w_basic_row_complete_coverage.xml ├── include │ └── opennav_coverage_bt │ │ ├── cancel_complete_coverage_path.hpp │ │ ├── compute_complete_coverage_path.hpp │ │ └── utils.hpp ├── package.xml ├── src │ ├── cancel_complete_coverage_path.cpp │ └── compute_complete_coverage_path.cpp └── test │ ├── CMakeLists.txt │ ├── test_cancel_complete_coverage.cpp │ └── test_compute_coverage_path.cpp ├── opennav_coverage_demo ├── README.md ├── launch │ ├── bringup_launch.py │ ├── coverage_demo_launch.py │ ├── row_bringup_launch.py │ └── row_coverage_demo_launch.py ├── opennav_coverage_demo │ ├── __init__.py │ ├── demo_coverage.py │ └── demo_row_coverage.py ├── package.xml ├── params │ ├── demo_params.yaml │ └── rviz_config.rviz ├── pytest.ini ├── resource │ └── opennav_coverage_demo ├── setup.cfg ├── setup.py ├── test │ ├── demo.png │ ├── demo_rows.png │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py └── world │ └── blank.world ├── opennav_coverage_msgs ├── CMakeLists.txt ├── README.md ├── action │ ├── ComputeCoveragePath.action │ └── NavigateCompleteCoverage.action ├── msg │ ├── Coordinate.msg │ ├── Coordinates.msg │ ├── HeadlandMode.msg │ ├── PathComponents.msg │ ├── PathMode.msg │ ├── RouteMode.msg │ ├── RowSwathMode.msg │ ├── Swath.msg │ └── SwathMode.msg └── package.xml ├── opennav_coverage_navigator ├── CMakeLists.txt ├── README.md ├── include │ └── opennav_coverage_navigator │ │ └── coverage_navigator.hpp ├── navigator_plugins.xml ├── package.xml ├── src │ └── coverage_navigator.cpp └── test │ ├── CMakeLists.txt │ └── test_coverage_navigator.cpp └── opennav_row_coverage ├── CMakeLists.txt ├── include └── opennav_row_coverage │ ├── row_coverage_server.hpp │ ├── row_swath_generator.hpp │ ├── types.hpp │ └── utils.hpp ├── package.xml ├── src ├── main.cpp ├── row_coverage_server.cpp └── row_swath_generator.cpp └── test ├── CMakeLists.txt ├── test_server.cpp ├── test_swath_generator.cpp ├── test_utils.cpp └── tester.py /.github/deps.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | Fields2Cover: 3 | type: git 4 | url: https://github.com/Fields2Cover/Fields2Cover.git 5 | version: v1.2.1-devel 6 | -------------------------------------------------------------------------------- /.github/workflows/lint.yml: -------------------------------------------------------------------------------- 1 | name: Lint 2 | on: 3 | pull_request: 4 | 5 | jobs: 6 | ament_lint_general: 7 | name: ament_${{ matrix.linter }} 8 | runs-on: ubuntu-latest 9 | container: 10 | image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest 11 | strategy: 12 | fail-fast: false 13 | matrix: 14 | linter: [copyright, xmllint, cpplint, uncrustify, pep257, flake8] 15 | steps: 16 | - uses: actions/checkout@v2 17 | - uses: ros-tooling/action-ros-lint@v0.1 18 | with: 19 | linter: ${{ matrix.linter }} 20 | distribution: jazzy 21 | package-name: | 22 | opennav_coverage 23 | opennav_coverage_bt 24 | opennav_coverage_demo 25 | opennav_coverage_msgs 26 | opennav_coverage_navigator 27 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: Test 2 | on: 3 | pull_request: 4 | push: 5 | branches: 6 | - main 7 | 8 | jobs: 9 | build_and_test: 10 | runs-on: ubuntu-latest 11 | env: 12 | ROS_DISTRO: ${{ matrix.ros_distro }} 13 | RMW_IMPLEMENTATION: rmw_cyclonedds_cpp 14 | container: 15 | image: rostooling/setup-ros-docker:ubuntu-noble-latest 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | ros_distro: [jazzy] 20 | steps: 21 | - uses: actions/checkout@v2 22 | - name: Install Cyclone DDS 23 | run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y 24 | - name: Build and run tests 25 | id: action-ros-ci 26 | uses: ros-tooling/action-ros-ci@v0.3 27 | with: 28 | import-token: ${{ secrets.GITHUB_TOKEN }} 29 | target-ros2-distro: ${{ matrix.ros_distro }} 30 | vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos" 31 | - uses: actions/upload-artifact@v4 32 | with: 33 | name: colcon-logs 34 | path: ros_ws/log 35 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Colcon output 32 | build 33 | log 34 | install 35 | 36 | # Visual Studio Code files 37 | .vscode 38 | 39 | # Eclipse project files 40 | .cproject 41 | .project 42 | .pydevproject 43 | 44 | # Python artifacts 45 | __pycache__/ 46 | *.py[cod] 47 | .ipynb_checkpoints 48 | 49 | sphinx_doc/_build 50 | 51 | # CLion artifacts 52 | .idea 53 | cmake-build-debug/ 54 | 55 | # doxygen docs 56 | doc/html/ 57 | -------------------------------------------------------------------------------- /opennav_coverage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(opennav_coverage) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(ament_index_cpp REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_action REQUIRED) 8 | find_package(rclcpp_lifecycle REQUIRED) 9 | find_package(rclcpp_components REQUIRED) 10 | find_package(std_msgs REQUIRED) 11 | find_package(visualization_msgs REQUIRED) 12 | find_package(nav2_util REQUIRED) 13 | find_package(nav2_msgs REQUIRED) 14 | find_package(nav_msgs REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(builtin_interfaces REQUIRED) 17 | find_package(tf2_ros REQUIRED) 18 | find_package(opennav_coverage_msgs REQUIRED) 19 | find_package(Fields2Cover REQUIRED) 20 | 21 | # potentially replace with nav2_common, nav2_package() 22 | set(CMAKE_CXX_STANDARD 17) 23 | add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) 24 | 25 | include_directories( 26 | include 27 | ) 28 | 29 | set(executable_name opennav_coverage) 30 | set(library_name ${executable_name}_core) 31 | 32 | set(dependencies 33 | ament_index_cpp 34 | rclcpp 35 | rclcpp_action 36 | rclcpp_lifecycle 37 | rclcpp_components 38 | std_msgs 39 | visualization_msgs 40 | nav2_util 41 | nav2_msgs 42 | nav_msgs 43 | geometry_msgs 44 | builtin_interfaces 45 | tf2_ros 46 | opennav_coverage_msgs 47 | ) 48 | 49 | add_library(${library_name} SHARED 50 | src/coverage_server.cpp 51 | src/headland_generator.cpp 52 | src/swath_generator.cpp 53 | src/route_generator.cpp 54 | src/path_generator.cpp 55 | src/visualizer.cpp 56 | ) 57 | 58 | ament_target_dependencies(${library_name} 59 | ${dependencies} 60 | ) 61 | 62 | target_link_libraries(${library_name} Fields2Cover) 63 | 64 | add_executable(${executable_name} 65 | src/main.cpp 66 | ) 67 | 68 | target_link_libraries(${executable_name} ${library_name}) 69 | 70 | ament_target_dependencies(${executable_name} 71 | ${dependencies} 72 | ) 73 | 74 | rclcpp_components_register_nodes(${library_name} "opennav_coverage::CoverageServer") 75 | 76 | install(TARGETS ${library_name} 77 | ARCHIVE DESTINATION lib 78 | LIBRARY DESTINATION lib 79 | RUNTIME DESTINATION bin 80 | ) 81 | 82 | install(TARGETS ${executable_name} 83 | RUNTIME DESTINATION lib/${PROJECT_NAME} 84 | ) 85 | 86 | install(DIRECTORY include/ 87 | DESTINATION include/ 88 | ) 89 | 90 | install(FILES test/test_field.xml 91 | test/cartesian_test_field.xml 92 | test/irregular_test_field.xml 93 | DESTINATION share/${PROJECT_NAME}/ 94 | ) 95 | 96 | if(BUILD_TESTING) 97 | find_package(ament_lint_auto REQUIRED) 98 | find_package(ament_cmake_gtest REQUIRED) 99 | ament_lint_auto_find_test_dependencies() 100 | add_subdirectory(test) 101 | endif() 102 | 103 | ament_export_include_directories(include) 104 | ament_export_libraries(${library_name}) 105 | ament_export_dependencies(${dependencies}) 106 | ament_package() 107 | -------------------------------------------------------------------------------- /opennav_coverage/README.md: -------------------------------------------------------------------------------- 1 | # Open Navigation's Nav2 Complete Coverage Server 2 | 3 | This package contains the coverage server 4 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/coverage_server.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__COVERAGE_SERVER_HPP_ 16 | #define OPENNAV_COVERAGE__COVERAGE_SERVER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "fields2cover.h" // NOLINT 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_util/node_utils.hpp" 26 | #include "nav2_util/simple_action_server.hpp" 27 | #include "opennav_coverage/headland_generator.hpp" 28 | #include "opennav_coverage/swath_generator.hpp" 29 | #include "opennav_coverage/route_generator.hpp" 30 | #include "opennav_coverage/path_generator.hpp" 31 | #include "opennav_coverage/visualizer.hpp" 32 | 33 | namespace opennav_coverage 34 | { 35 | /** 36 | * @class opennav_coverage::CoverageServer 37 | * @brief An action server which implements highly reconfigurable complete 38 | * coverage planning using the Fields2Cover library 39 | */ 40 | class CoverageServer : public nav2_util::LifecycleNode 41 | { 42 | public: 43 | using ActionServer = nav2_util::SimpleActionServer; 44 | 45 | /** 46 | * @brief A constructor for opennav_coverage::CoverageServer 47 | * @param options Additional options to control creation of the node. 48 | */ 49 | explicit CoverageServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 50 | 51 | /** 52 | * @brief A destructor for opennav_coverage::CoverageServer 53 | */ 54 | ~CoverageServer() = default; 55 | 56 | protected: 57 | /** 58 | * @brief Main action callback method to complete action request 59 | */ 60 | void computeCoveragePath(); 61 | 62 | /** 63 | * @brief Validate the goal settings to know if valid to execute 64 | * @param Goal request to validate 65 | * @return SUCCESS or FAILURE 66 | */ 67 | bool validateGoal(typename std::shared_ptr req); 68 | 69 | /** 70 | * @brief Gets a preempted goal if immediately requested 71 | * @param Goal goal to check or replace if required with preemption 72 | * @return SUCCESS or FAILURE 73 | */ 74 | void getPreemptedGoalIfRequested( 75 | typename std::shared_ptr goal); 76 | 77 | /** 78 | * @brief Configure member variables 79 | * @param state Reference to LifeCycle node state 80 | * @return SUCCESS or FAILURE 81 | */ 82 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 83 | 84 | /** 85 | * @brief Activate member variables 86 | * @param state Reference to LifeCycle node state 87 | * @return SUCCESS or FAILURE 88 | */ 89 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 90 | 91 | /** 92 | * @brief Deactivate member variables 93 | * @param state Reference to LifeCycle node state 94 | * @return SUCCESS or FAILURE 95 | */ 96 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 97 | 98 | /** 99 | * @brief Reset member variables 100 | * @param state Reference to LifeCycle node state 101 | * @return SUCCESS or FAILURE 102 | */ 103 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 104 | 105 | /** 106 | * @brief Called when in shutdown state 107 | * @param state Reference to LifeCycle node state 108 | * @return SUCCESS or FAILURE 109 | */ 110 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 111 | 112 | /** 113 | * @brief Callback executed when a parameter change is detected 114 | * @param event ParameterEvent message 115 | */ 116 | rcl_interfaces::msg::SetParametersResult 117 | dynamicParametersCallback(std::vector parameters); 118 | 119 | // Dynamic parameters handler 120 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 121 | std::mutex dynamic_params_lock_; 122 | 123 | std::unique_ptr action_server_; 124 | 125 | std::unique_ptr robot_params_; 126 | std::unique_ptr headland_gen_; 127 | std::unique_ptr swath_gen_; 128 | std::unique_ptr route_gen_; 129 | std::unique_ptr path_gen_; 130 | std::unique_ptr visualizer_; 131 | bool cartesian_frame_; 132 | }; 133 | 134 | } // namespace opennav_coverage 135 | 136 | #endif // OPENNAV_COVERAGE__COVERAGE_SERVER_HPP_ 137 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/headland_generator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__HEADLAND_GENERATOR_HPP_ 16 | #define OPENNAV_COVERAGE__HEADLAND_GENERATOR_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "fields2cover.h" // NOLINT 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_util/node_utils.hpp" 26 | #include "opennav_coverage_msgs/msg/headland_mode.hpp" 27 | #include "opennav_coverage/utils.hpp" 28 | #include "opennav_coverage/types.hpp" 29 | 30 | namespace opennav_coverage 31 | { 32 | 33 | /** 34 | * @class Headland mode and state 35 | */ 36 | class HeadlandGenerator 37 | { 38 | public: 39 | /** 40 | * @brief Constructor for Headland mode 41 | * @param node A node to get the Headland type from 42 | */ 43 | template 44 | explicit HeadlandGenerator(const NodeT & node) 45 | { 46 | logger_ = node->get_logger(); 47 | 48 | nav2_util::declare_parameter_if_not_declared( 49 | node, "default_headland_type", rclcpp::ParameterValue("CONSTANT")); 50 | std::string type_str = node->get_parameter("default_headland_type").as_string(); 51 | default_type_ = toType(type_str); 52 | default_generator_ = createGenerator(default_type_); 53 | 54 | nav2_util::declare_parameter_if_not_declared( 55 | node, "default_headland_width", rclcpp::ParameterValue(2.0)); 56 | default_headland_width_ = node->get_parameter("default_headland_width").as_double(); 57 | } 58 | 59 | /** 60 | * @brief Main method to generate headlands 61 | * @param field Field to generate headlands from 62 | * @param settings Action request information 63 | */ 64 | Field generateHeadlands( 65 | const Field & field, const opennav_coverage_msgs::msg::HeadlandMode & settings); 66 | 67 | /** 68 | * @brief Sets the mode manually of the Headland for dynamic parameters 69 | * @param mode String for mode to use 70 | */ 71 | void setMode(const std::string & new_mode); 72 | 73 | /** 74 | * @brief Sets the headland width manually for dynamic parameters 75 | * @param mode String for mode to use 76 | */ 77 | void setWidth(const double & width) {default_headland_width_ = width;} 78 | 79 | protected: 80 | /** 81 | * @brief Creates generator pointer of a requested type 82 | * @param type Headland generator type to create 83 | * @return Generator to use 84 | */ 85 | HeadlandGeneratorPtr createGenerator(const HeadlandType & type); 86 | 87 | /** 88 | * @brief Converts the Headland mode into a string for publication 89 | * @param Type of mode 90 | * @return String of mode 91 | */ 92 | std::string toString(const HeadlandType & type); 93 | 94 | /** 95 | * @brief Converts the Headland string into a mode for handling 96 | * @param String of mode 97 | * @return Type of mode 98 | */ 99 | HeadlandType toType(const std::string & str); 100 | 101 | HeadlandType default_type_; 102 | double default_headland_width_; 103 | HeadlandGeneratorPtr default_generator_{nullptr}; 104 | rclcpp::Logger logger_{rclcpp::get_logger("HeadlandGenerator")}; 105 | }; 106 | 107 | } // namespace opennav_coverage 108 | 109 | #endif // OPENNAV_COVERAGE__HEADLAND_GENERATOR_HPP_ 110 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/path_generator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__PATH_GENERATOR_HPP_ 16 | #define OPENNAV_COVERAGE__PATH_GENERATOR_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "fields2cover.h" // NOLINT 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "nav2_util/lifecycle_node.hpp" 26 | #include "nav2_util/node_utils.hpp" 27 | #include "opennav_coverage_msgs/msg/path_mode.hpp" 28 | #include "opennav_coverage/utils.hpp" 29 | #include "opennav_coverage/types.hpp" 30 | #include "opennav_coverage/robot_params.hpp" 31 | 32 | namespace opennav_coverage 33 | { 34 | 35 | /** 36 | * @class Path mode and state 37 | */ 38 | class PathGenerator 39 | { 40 | public: 41 | /** 42 | * @brief Constructor for swath mode 43 | * @param node A node to get the swath type from 44 | */ 45 | template 46 | explicit PathGenerator(const NodeT & node, RobotParams * robot_params) 47 | { 48 | logger_ = node->get_logger(); 49 | robot_params_ = robot_params; 50 | 51 | nav2_util::declare_parameter_if_not_declared( 52 | node, "default_path_type", rclcpp::ParameterValue("DUBIN")); 53 | std::string type_str = node->get_parameter("default_path_type").as_string(); 54 | default_type_ = toType(type_str); 55 | 56 | nav2_util::declare_parameter_if_not_declared( 57 | node, "default_path_continuity_type", rclcpp::ParameterValue("CONTINUOUS")); 58 | std::string type_cont_str = node->get_parameter("default_path_continuity_type").as_string(); 59 | default_continuity_type_ = toContinuityType(type_cont_str); 60 | 61 | nav2_util::declare_parameter_if_not_declared( 62 | node, "default_turn_point_distance", rclcpp::ParameterValue(0.1)); 63 | default_turn_point_distance_ = node->get_parameter("default_turn_point_distance").as_double(); 64 | 65 | // Path Generator requires no changes at runtime 66 | generator_ = std::make_unique(); 67 | default_curve_ = createCurve(default_type_, default_continuity_type_); 68 | } 69 | 70 | /** 71 | * @brief Main method to generate path 72 | * @param Swaths swaths to generate path from 73 | * @param request Action request information 74 | * @return Path complete path 75 | */ 76 | Path generatePath( 77 | const Swaths & swaths, const opennav_coverage_msgs::msg::PathMode & settings); 78 | 79 | /** 80 | * @brief Sets the mode manually of the paths for dynamic parameters 81 | * @param mode String for mode to use 82 | */ 83 | void setPathMode(const std::string & new_mode); 84 | 85 | /** 86 | * @brief Sets the mode manually of the path continuity for dynamic parameters 87 | * @param mode String for mode to use 88 | */ 89 | void setPathContinuityMode(const std::string & new_mode); 90 | 91 | /** 92 | * @brief Sets the mode manually of the density of path points in turns 93 | * @param mode double for mode to use 94 | */ 95 | void setTurnPointDistance(const double & setting) {default_turn_point_distance_ = setting;} 96 | 97 | float getTurnPointDistance() {return default_turn_point_distance_;} 98 | 99 | protected: 100 | /** 101 | * @brief Creates generator pointer of a requested type 102 | * @param type curve generator type to create 103 | * @return Generator to use 104 | */ 105 | TurningBasePtr createCurve(const PathType & type, const PathContinuityType & c_type); 106 | 107 | /** 108 | * @brief Converts the path mode into a string for publication 109 | * @return String of mode 110 | */ 111 | std::string toString(const PathType & type, const PathContinuityType & c_type); 112 | 113 | /** 114 | * @brief Converts the Path string into a mode for handling 115 | * @param String of mode 116 | * @return Type of mode 117 | */ 118 | PathType toType(const std::string & str); 119 | 120 | /** 121 | * @brief Converts the Path string into a mode for handling 122 | * @param String of mode 123 | * @return Type of mode 124 | */ 125 | PathContinuityType toContinuityType(const std::string & str); 126 | 127 | PathType default_type_; 128 | PathContinuityType default_continuity_type_; 129 | float default_turn_point_distance_; 130 | TurningBasePtr default_curve_; 131 | std::unique_ptr generator_; 132 | RobotParams * robot_params_; 133 | rclcpp::Logger logger_{rclcpp::get_logger("SwathGenerator")}; 134 | }; 135 | 136 | } // namespace opennav_coverage 137 | 138 | #endif // OPENNAV_COVERAGE__PATH_GENERATOR_HPP_ 139 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/robot_params.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__ROBOT_PARAMS_HPP_ 16 | #define OPENNAV_COVERAGE__ROBOT_PARAMS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "fields2cover.h" // NOLINT 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_util/node_utils.hpp" 26 | #include "opennav_coverage/utils.hpp" 27 | #include "opennav_coverage/types.hpp" 28 | 29 | namespace opennav_coverage 30 | { 31 | 32 | /** 33 | * @class Robot's mode and state 34 | */ 35 | class RobotParams 36 | { 37 | public: 38 | /** 39 | * @brief Constructor for robot mode 40 | * @param node A node to get the robot parameters from 41 | */ 42 | template 43 | explicit RobotParams(const NodeT & node) 44 | { 45 | nav2_util::declare_parameter_if_not_declared( 46 | node, "robot_width", rclcpp::ParameterValue(2.1)); 47 | robot_.robot_width = node->get_parameter("robot_width").as_double(); 48 | 49 | nav2_util::declare_parameter_if_not_declared( 50 | node, "operation_width", rclcpp::ParameterValue(2.5)); 51 | robot_.op_width = node->get_parameter("operation_width").as_double(); 52 | 53 | nav2_util::declare_parameter_if_not_declared( 54 | node, "min_turning_radius", rclcpp::ParameterValue(0.4)); 55 | robot_.setMinRadius(node->get_parameter("min_turning_radius").as_double()); 56 | 57 | nav2_util::declare_parameter_if_not_declared( 58 | node, "linear_curv_change", rclcpp::ParameterValue(2.0)); 59 | robot_.linear_curv_change = node->get_parameter("linear_curv_change").as_double(); 60 | } 61 | 62 | /** 63 | * @brief Get the robot's width 64 | * @return robot width in m 65 | */ 66 | double getWidth() 67 | { 68 | return robot_.robot_width; 69 | } 70 | 71 | /** 72 | * @brief Get the robot's operational width 73 | * @return robot's operational width in m 74 | */ 75 | double getOperationWidth() 76 | { 77 | return robot_.op_width; 78 | } 79 | 80 | /** 81 | * @brief Get the robot's F2C model 82 | * @return Robot F2C typed model 83 | */ 84 | Robot & getRobot() 85 | { 86 | return robot_; 87 | } 88 | 89 | Robot robot_; 90 | }; 91 | 92 | } // namespace opennav_coverage 93 | 94 | #endif // OPENNAV_COVERAGE__ROBOT_PARAMS_HPP_ 95 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/route_generator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__ROUTE_GENERATOR_HPP_ 16 | #define OPENNAV_COVERAGE__ROUTE_GENERATOR_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "fields2cover.h" // NOLINT 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_util/node_utils.hpp" 26 | #include "opennav_coverage_msgs/msg/route_mode.hpp" 27 | #include "opennav_coverage/utils.hpp" 28 | #include "opennav_coverage/types.hpp" 29 | 30 | namespace opennav_coverage 31 | { 32 | 33 | /** 34 | * @class Route mode and state 35 | */ 36 | class RouteGenerator 37 | { 38 | public: 39 | /** 40 | * @brief Constructor for swath mode 41 | * @param node A node to get the swath type from 42 | */ 43 | template 44 | explicit RouteGenerator(const NodeT & node) 45 | { 46 | nav2_util::declare_parameter_if_not_declared( 47 | node, "default_route_type", rclcpp::ParameterValue("BOUSTROPHEDON")); 48 | std::string type_str = node->get_parameter("default_route_type").as_string(); 49 | default_type_ = toType(type_str); 50 | default_generator_ = createGenerator(default_type_); 51 | 52 | nav2_util::declare_parameter_if_not_declared( 53 | node, "default_spiral_n", rclcpp::ParameterValue(4)); 54 | default_spiral_n_ = node->get_parameter("default_spiral_n").as_int(); 55 | 56 | nav2_util::declare_parameter_if_not_declared( 57 | node, "default_custom_order", rclcpp::PARAMETER_INTEGER_ARRAY); 58 | try { 59 | // Get the custom order and cast to size_t 60 | auto order = node->get_parameter("default_custom_order").as_integer_array(); 61 | default_custom_order_ = std::vector(order.begin(), order.end()); 62 | } catch (...) { 63 | RCLCPP_WARN( 64 | node->get_logger(), 65 | "default_custom_order was not set! " 66 | "If using Custom Route mode, the custom order must be set per-request!"); 67 | } 68 | } 69 | 70 | /** 71 | * @brief Main method to generate route 72 | * @param Swaths swaths to generate route from 73 | * @param request Action request information 74 | * @return Swaths ordered swaths 75 | */ 76 | Swaths generateRoute( 77 | const Swaths & swaths, const opennav_coverage_msgs::msg::RouteMode & settings); 78 | 79 | /** 80 | * @brief Sets the mode manually of the Route for dynamic parameters 81 | * @param mode String for mode to use 82 | */ 83 | void setMode(const std::string & new_mode); 84 | 85 | /** 86 | * @brief Sets the spiral N manually for dynamic parameters 87 | * @param mode String for mode to use 88 | */ 89 | void setSpiralN(const int & n) {default_spiral_n_ = n;} 90 | 91 | /** 92 | * @brief Sets the custom order manually for dynamic parameters 93 | * @param mode String for mode to use 94 | */ 95 | void setCustomOrder(const std::vector & order) // NOLINT 96 | { 97 | default_custom_order_ = std::vector(order.begin(), order.end()); 98 | } 99 | 100 | protected: 101 | /** 102 | * @brief Creates generator pointer of a requested type 103 | * @param type Route generator type to create 104 | * @return Generator to use 105 | */ 106 | RouteGeneratorPtr createGenerator(const RouteType & type); 107 | 108 | /** 109 | * @brief Converts the route mode into a string for publication 110 | * @param Type of mode 111 | * @return String of mode 112 | */ 113 | std::string toString(const RouteType & type); 114 | 115 | /** 116 | * @brief Converts the route string into a mode for handling 117 | * @param String of mode 118 | * @return Type of mode 119 | */ 120 | RouteType toType(const std::string & str); 121 | 122 | RouteType default_type_; 123 | std::vector default_custom_order_; 124 | size_t default_spiral_n_; 125 | RouteGeneratorPtr default_generator_{nullptr}; 126 | rclcpp::Logger logger_{rclcpp::get_logger("RouteGenerator")}; 127 | }; 128 | 129 | } // namespace opennav_coverage 130 | 131 | #endif // OPENNAV_COVERAGE__ROUTE_GENERATOR_HPP_ 132 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/swath_generator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__SWATH_GENERATOR_HPP_ 16 | #define OPENNAV_COVERAGE__SWATH_GENERATOR_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "fields2cover.h" // NOLINT 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "nav2_util/lifecycle_node.hpp" 26 | #include "nav2_util/node_utils.hpp" 27 | #include "opennav_coverage_msgs/msg/swath_mode.hpp" 28 | #include "opennav_coverage/utils.hpp" 29 | #include "opennav_coverage/types.hpp" 30 | #include "opennav_coverage/robot_params.hpp" 31 | 32 | namespace opennav_coverage 33 | { 34 | 35 | /** 36 | * @class Swath mode and state 37 | */ 38 | class SwathGenerator 39 | { 40 | public: 41 | /** 42 | * @brief Constructor for swath mode 43 | * @param node A node to get the swath type from 44 | */ 45 | template 46 | explicit SwathGenerator(const NodeT & node, RobotParams * robot_params) 47 | { 48 | logger_ = node->get_logger(); 49 | robot_params_ = robot_params; 50 | 51 | nav2_util::declare_parameter_if_not_declared( 52 | node, "default_swath_type", rclcpp::ParameterValue("LENGTH")); 53 | std::string type_str = node->get_parameter("default_swath_type").as_string(); 54 | default_type_ = toType(type_str); 55 | 56 | nav2_util::declare_parameter_if_not_declared( 57 | node, "default_swath_angle_type", rclcpp::ParameterValue("BRUTE_FORCE")); 58 | std::string angle_str = node->get_parameter("default_swath_angle_type").as_string(); 59 | default_angle_type_ = toAngleType(angle_str); 60 | 61 | nav2_util::declare_parameter_if_not_declared( 62 | node, "default_swath_angle", rclcpp::ParameterValue(0.0)); 63 | default_swath_angle_ = node->get_parameter("default_swath_angle").as_double(); 64 | 65 | nav2_util::declare_parameter_if_not_declared( 66 | node, "default_step_angle", rclcpp::ParameterValue(1.7453e-2)); 67 | default_step_angle_ = node->get_parameter("default_step_angle").as_double(); 68 | 69 | nav2_util::declare_parameter_if_not_declared( 70 | node, "default_allow_overlap", rclcpp::ParameterValue(false)); 71 | default_allow_overlap_ = node->get_parameter("default_allow_overlap").as_bool(); 72 | 73 | // Swath Generator BruteForce can also accept non-brute force specific 74 | // angle requests from SwathGeneratorBase, thus no need to change generators 75 | generator_ = std::make_unique(); 76 | default_objective_ = createObjective(default_type_); 77 | } 78 | 79 | /** 80 | * @brief Main method to generate swaths in field for cell 81 | * @param field Field to generate swaths from 82 | * @param request Action request information 83 | */ 84 | Swaths generateSwaths( 85 | const Field & field, const opennav_coverage_msgs::msg::SwathMode & settings); 86 | 87 | /** 88 | * @brief Sets the mode manually of the swath for dynamic parameters 89 | * @param mode String for mode to use 90 | */ 91 | void setSwathMode(const std::string & new_mode); 92 | 93 | /** 94 | * @brief Sets the mode manually of the swath angle for dynamic parameters 95 | * @param mode String for mode to use 96 | */ 97 | void setSwathAngleMode(const std::string & new_mode); 98 | 99 | /** 100 | * @brief Sets the best swath angle manually for dynamic parameters 101 | * @param mode String for mode to use 102 | */ 103 | void setSwathAngle(const double & best_angle) {default_swath_angle_ = best_angle;} 104 | 105 | /** 106 | * @brief Sets default overlap allowed for dynamic parameters 107 | * @param mode String for mode to use 108 | */ 109 | void setOVerlap(const bool & setting) {default_allow_overlap_ = setting;} 110 | 111 | /** 112 | * @brief Sets default search step for brute force for dynamic parameters 113 | * @param mode String for mode to use 114 | */ 115 | void setStepAngle(const bool & setting) {default_step_angle_ = setting;} 116 | 117 | protected: 118 | /** 119 | * @brief Creates objective pointer of a requested type 120 | * @param type Swaths generator type to create 121 | * @return Generator to use 122 | */ 123 | SwathObjectivePtr createObjective(const SwathType & type); 124 | 125 | /** 126 | * @brief Converts the swatch mode into a string for publication 127 | * @param Swath type 128 | * @param Swath angle type 129 | * @return String of mode 130 | */ 131 | std::string toString(const SwathType & type, const SwathAngleType & angle_type); 132 | 133 | /** 134 | * @brief Converts the swatch string into a type for handling 135 | * @param str Type string to convert 136 | * @return Type of mode 137 | */ 138 | SwathType toType(const std::string & str); 139 | 140 | /** 141 | * @brief Converts the swatch angle string into a type for handling 142 | * @param str Type string to convert 143 | * @return Type of mode 144 | */ 145 | SwathAngleType toAngleType(const std::string & str); 146 | 147 | SwathType default_type_; 148 | SwathAngleType default_angle_type_; 149 | SwathObjectivePtr default_objective_; 150 | double default_swath_angle_; 151 | double default_step_angle_; 152 | bool default_allow_overlap_; 153 | std::unique_ptr generator_; 154 | RobotParams * robot_params_; 155 | rclcpp::Logger logger_{rclcpp::get_logger("SwathGenerator")}; 156 | }; 157 | 158 | } // namespace opennav_coverage 159 | 160 | #endif // OPENNAV_COVERAGE__SWATH_GENERATOR_HPP_ 161 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__TYPES_HPP_ 16 | #define OPENNAV_COVERAGE__TYPES_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "fields2cover.h" // NOLINT 23 | 24 | #include "opennav_coverage_msgs/action/compute_coverage_path.hpp" 25 | 26 | namespace opennav_coverage 27 | { 28 | 29 | typedef F2CCells Fields; 30 | typedef F2CCell Field; 31 | typedef F2CSwaths Swaths; 32 | typedef F2CSwath Swath; 33 | typedef F2CPath Path; 34 | typedef F2CRobot Robot; 35 | typedef F2CLinearRing Polygon; 36 | typedef F2CPoint Point; 37 | typedef f2c::types::PathState PathState; 38 | typedef f2c::types::LineString LineString; 39 | typedef F2CLineString LineString; 40 | 41 | typedef std::shared_ptr HeadlandGeneratorPtr; 42 | typedef std::shared_ptr SwathObjectivePtr; 43 | typedef std::shared_ptr TurningBasePtr; 44 | typedef std::shared_ptr RouteGeneratorPtr; 45 | 46 | typedef opennav_coverage_msgs::action::ComputeCoveragePath ComputeCoveragePath; 47 | 48 | /** 49 | * @enum Headland computations types 50 | */ 51 | enum class HeadlandType 52 | { 53 | UNKNOWN = 0, 54 | CONSTANT = 1 55 | }; 56 | 57 | /** 58 | * @enum Swath computations types 59 | */ 60 | enum class SwathType 61 | { 62 | UNKNOWN = 0, 63 | LENGTH = 1, 64 | NUMBER = 2, 65 | COVERAGE = 3 66 | }; 67 | 68 | /** 69 | * @enum Swath angle types 70 | */ 71 | enum class SwathAngleType 72 | { 73 | UNKNOWN = 0, 74 | SET_ANGLE = 1, 75 | BRUTE_FORCE = 2 76 | }; 77 | 78 | /** 79 | * @enum Swath computations types 80 | */ 81 | enum class RouteType 82 | { 83 | UNKNOWN = 0, 84 | BOUSTROPHEDON = 1, 85 | SNAKE = 2, 86 | SPIRAL = 3, 87 | CUSTOM = 4 88 | }; 89 | 90 | /** 91 | * @enum Path computations types 92 | */ 93 | enum class PathType 94 | { 95 | UNKNOWN = 0, 96 | REEDS_SHEPP = 1, 97 | DUBIN = 2, 98 | }; 99 | 100 | /** 101 | * @enum Path continuity types 102 | */ 103 | enum class PathContinuityType 104 | { 105 | UNKNOWN = 0, 106 | CONTINUOUS = 1, 107 | DISCONTINUOUS = 2 108 | }; 109 | 110 | class CoverageException : public std::runtime_error 111 | { 112 | public: 113 | explicit CoverageException(const std::string & description) 114 | : std::runtime_error(description) {} 115 | }; 116 | 117 | } // namespace opennav_coverage 118 | 119 | #endif // OPENNAV_COVERAGE__TYPES_HPP_ 120 | -------------------------------------------------------------------------------- /opennav_coverage/include/opennav_coverage/visualizer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE__VISUALIZER_HPP_ 16 | #define OPENNAV_COVERAGE__VISUALIZER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "opennav_coverage/types.hpp" 25 | #include "opennav_coverage/utils.hpp" 26 | #include "nav2_util/geometry_utils.hpp" 27 | 28 | #include "geometry_msgs/msg/polygon_stamped.hpp" 29 | #include "nav_msgs/msg/path.hpp" 30 | #include "visualization_msgs/msg/marker.hpp" 31 | 32 | namespace opennav_coverage 33 | { 34 | 35 | class Visualizer 36 | { 37 | public: 38 | /** 39 | * @brief Constructor for visualizations 40 | * @param node A node to create interfaces from 41 | */ 42 | Visualizer() = default; 43 | 44 | template 45 | void activate(NodeT node) 46 | { 47 | nav_plan_pub_ = rclcpp::create_publisher( 48 | node->get_node_topics_interface(), 49 | "coverage_server/coverage_plan", rclcpp::QoS(1)); 50 | headlands_pub_ = rclcpp::create_publisher( 51 | node->get_node_topics_interface(), 52 | "coverage_server/field_boundary", rclcpp::QoS(1)); 53 | planning_field_pub_ = rclcpp::create_publisher( 54 | node->get_node_topics_interface(), 55 | "coverage_server/planning_field", rclcpp::QoS(1)); 56 | swaths_pub_ = rclcpp::create_publisher( 57 | node->get_node_topics_interface(), 58 | "coverage_server/swaths", rclcpp::QoS(1)); 59 | } 60 | 61 | void deactivate(); 62 | 63 | void visualize( 64 | const Field & total_field, const Field & no_headland_field, 65 | const Point & ref_pt, const nav_msgs::msg::Path & path, 66 | const Swaths swaths, const std_msgs::msg::Header & header); 67 | 68 | rclcpp::Publisher::SharedPtr nav_plan_pub_; 69 | rclcpp::Publisher::SharedPtr headlands_pub_; 70 | rclcpp::Publisher::SharedPtr planning_field_pub_; 71 | rclcpp::Publisher::SharedPtr swaths_pub_; 72 | }; 73 | 74 | } // namespace opennav_coverage 75 | 76 | #endif // OPENNAV_COVERAGE__VISUALIZER_HPP_ 77 | -------------------------------------------------------------------------------- /opennav_coverage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opennav_coverage 5 | 0.0.1 6 | A Task Server for complete coverage planning 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | ament_index_cpp 13 | rclcpp 14 | rclcpp_action 15 | rclcpp_lifecycle 16 | nav2_util 17 | nav2_msgs 18 | nav_msgs 19 | geometry_msgs 20 | builtin_interfaces 21 | tf2_ros 22 | fields2cover 23 | opennav_coverage_msgs 24 | 25 | ament_lint_common 26 | ament_lint_auto 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /opennav_coverage/src/headland_generator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "opennav_coverage/headland_generator.hpp" 19 | 20 | namespace opennav_coverage 21 | { 22 | 23 | Field HeadlandGenerator::generateHeadlands( 24 | const Field & field, const opennav_coverage_msgs::msg::HeadlandMode & settings) 25 | { 26 | HeadlandType action_type = toType(settings.mode); 27 | HeadlandGeneratorPtr generator{nullptr}; 28 | double width = 0.0; 29 | 30 | // If not set by action, use default mode 31 | if (action_type == HeadlandType::UNKNOWN) { 32 | action_type = default_type_; 33 | generator = default_generator_; 34 | width = default_headland_width_; 35 | } else { 36 | generator = createGenerator(action_type); 37 | width = settings.width; 38 | } 39 | 40 | 41 | if (!generator) { 42 | throw CoverageException("No valid headlands mode set! Options: CONSTANT."); 43 | } 44 | 45 | RCLCPP_DEBUG(logger_, "Generating Headland with generator: %s", toString(action_type).c_str()); 46 | return generator->generateHeadlands(Fields(field), width).getGeometry(0); 47 | } 48 | 49 | void HeadlandGenerator::setMode(const std::string & new_mode) 50 | { 51 | default_type_ = toType(new_mode); 52 | default_generator_ = createGenerator(default_type_); 53 | } 54 | 55 | HeadlandGeneratorPtr HeadlandGenerator::createGenerator(const HeadlandType & type) 56 | { 57 | switch (type) { 58 | case HeadlandType::CONSTANT: 59 | return std::move(std::make_shared()); 60 | default: 61 | RCLCPP_WARN(logger_, "Unknown headland type set!"); 62 | return HeadlandGeneratorPtr{nullptr}; 63 | } 64 | } 65 | 66 | std::string HeadlandGenerator::toString(const HeadlandType & type) 67 | { 68 | switch (type) { 69 | case HeadlandType::CONSTANT: 70 | return "Constant"; 71 | default: 72 | return "Unknown"; 73 | } 74 | } 75 | 76 | HeadlandType HeadlandGenerator::toType(const std::string & str) 77 | { 78 | std::string mode_str = str; 79 | util::toUpper(mode_str); 80 | if (mode_str == "CONSTANT") { 81 | return HeadlandType::CONSTANT; 82 | } else { 83 | return HeadlandType::UNKNOWN; 84 | } 85 | } 86 | 87 | } // namespace opennav_coverage 88 | -------------------------------------------------------------------------------- /opennav_coverage/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "opennav_coverage/coverage_server.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /opennav_coverage/src/path_generator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "opennav_coverage/path_generator.hpp" 19 | 20 | namespace opennav_coverage 21 | { 22 | 23 | Path PathGenerator::generatePath( 24 | const Swaths & swaths, const opennav_coverage_msgs::msg::PathMode & settings) 25 | { 26 | PathType action_type = toType(settings.mode); 27 | PathContinuityType action_continuity_type = toContinuityType(settings.continuity_mode); 28 | std::shared_ptr curve{nullptr}; 29 | float turn_point_distance; 30 | 31 | // If not set by action, use default mode 32 | if (action_type == PathType::UNKNOWN || action_continuity_type == PathContinuityType::UNKNOWN) { 33 | action_type = default_type_; 34 | action_continuity_type = default_continuity_type_; 35 | curve = default_curve_; 36 | turn_point_distance = default_turn_point_distance_; 37 | } else { 38 | curve = createCurve(action_type, action_continuity_type); 39 | turn_point_distance = settings.turn_point_distance; 40 | } 41 | 42 | if (!curve) { 43 | throw CoverageException("No valid path mode set!"); 44 | } 45 | 46 | RCLCPP_DEBUG( 47 | logger_, 48 | "Generating path with curve: %s", toString(action_type, action_continuity_type).c_str()); 49 | generator_->turn_point_dist = turn_point_distance; 50 | return generator_->searchBestPath(robot_params_->getRobot(), swaths, *curve); 51 | } 52 | 53 | void PathGenerator::setPathMode(const std::string & new_mode) 54 | { 55 | default_type_ = toType(new_mode); 56 | default_curve_ = createCurve(default_type_, default_continuity_type_); 57 | } 58 | 59 | void PathGenerator::setPathContinuityMode(const std::string & new_mode) 60 | { 61 | default_continuity_type_ = toContinuityType(new_mode); 62 | default_curve_ = createCurve(default_type_, default_continuity_type_); 63 | } 64 | 65 | TurningBasePtr PathGenerator::createCurve(const PathType & type, const PathContinuityType & c_type) 66 | { 67 | switch (type) { 68 | case PathType::DUBIN: 69 | if (c_type == PathContinuityType::CONTINUOUS) { 70 | return std::move(std::make_shared()); 71 | } else if (c_type == PathContinuityType::DISCONTINUOUS) { 72 | return std::move(std::make_shared()); 73 | } 74 | RCLCPP_WARN(logger_, "Unknown continuity type set! Options: CONTINUOUS, DISCONTINUOUS."); 75 | return TurningBasePtr{nullptr}; 76 | case PathType::REEDS_SHEPP: 77 | if (c_type == PathContinuityType::CONTINUOUS) { 78 | return std::move(std::make_shared()); 79 | } else if (c_type == PathContinuityType::DISCONTINUOUS) { 80 | return std::move(std::make_shared()); 81 | } 82 | RCLCPP_WARN(logger_, "Unknown continuity type set! Options: CONTINUOUS, DISCONTINUOUS."); 83 | return TurningBasePtr{nullptr}; 84 | default: 85 | RCLCPP_WARN(logger_, "Unknown path type set! Options: DUBIN, REEDS_SHEPP."); 86 | return TurningBasePtr{nullptr}; 87 | } 88 | } 89 | 90 | std::string PathGenerator::toString(const PathType & type, const PathContinuityType & c_type) 91 | { 92 | std::string str; 93 | switch (type) { 94 | case PathType::DUBIN: 95 | str = "Dubin"; 96 | break; 97 | case PathType::REEDS_SHEPP: 98 | str = "Reeds-Shepp"; 99 | break; 100 | default: 101 | str = "Unknown"; 102 | break; 103 | } 104 | 105 | str += " Mode and "; 106 | 107 | switch (c_type) { 108 | case PathContinuityType::CONTINUOUS: 109 | str += "Continuous"; 110 | break; 111 | case PathContinuityType::DISCONTINUOUS: 112 | str += "Discontinuous"; 113 | break; 114 | default: 115 | str += "Unknown"; 116 | break; 117 | } 118 | 119 | str += " connections."; 120 | return str; 121 | } 122 | 123 | PathType PathGenerator::toType(const std::string & str) 124 | { 125 | std::string mode_str = str; 126 | util::toUpper(mode_str); 127 | if (mode_str == "REEDS_SHEPP") { 128 | return PathType::REEDS_SHEPP; 129 | } else if (mode_str == "DUBIN") { 130 | return PathType::DUBIN; 131 | } else { 132 | return PathType::UNKNOWN; 133 | } 134 | } 135 | 136 | PathContinuityType PathGenerator::toContinuityType(const std::string & str) 137 | { 138 | std::string mode_str = str; 139 | util::toUpper(mode_str); 140 | if (mode_str == "CONTINUOUS") { 141 | return PathContinuityType::CONTINUOUS; 142 | } else if (mode_str == "DISCONTINUOUS") { 143 | return PathContinuityType::DISCONTINUOUS; 144 | } else { 145 | return PathContinuityType::UNKNOWN; 146 | } 147 | } 148 | 149 | } // namespace opennav_coverage 150 | -------------------------------------------------------------------------------- /opennav_coverage/src/route_generator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "opennav_coverage/route_generator.hpp" 19 | 20 | namespace opennav_coverage 21 | { 22 | 23 | Swaths RouteGenerator::generateRoute( 24 | const Swaths & swaths, const opennav_coverage_msgs::msg::RouteMode & settings) 25 | { 26 | RouteType action_type = toType(settings.mode); 27 | std::shared_ptr generator{nullptr}; 28 | size_t spiral_n; 29 | std::vector custom_order; 30 | 31 | // If not set by action, use default mode 32 | if (action_type == RouteType::UNKNOWN) { 33 | action_type = default_type_; 34 | generator = default_generator_; 35 | spiral_n = default_spiral_n_; 36 | custom_order = default_custom_order_; 37 | } else { 38 | generator = createGenerator(action_type); 39 | spiral_n = settings.spiral_n; 40 | custom_order = std::vector(settings.custom_order.begin(), settings.custom_order.end()); 41 | } 42 | 43 | if (!generator) { 44 | throw CoverageException( 45 | "No valid route mode set! Options: BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM."); 46 | } else if (action_type == RouteType::SPIRAL) { 47 | dynamic_cast(generator.get())->setSpiralSize(spiral_n); 48 | } else if (action_type == RouteType::CUSTOM) { 49 | dynamic_cast(generator.get())->setCustomOrder(custom_order); 50 | } 51 | 52 | RCLCPP_DEBUG(logger_, "Generating route with generator: %s", toString(action_type).c_str()); 53 | return generator->genSortedSwaths(swaths); 54 | } 55 | 56 | void RouteGenerator::setMode(const std::string & new_mode) 57 | { 58 | default_type_ = toType(new_mode); 59 | default_generator_ = createGenerator(default_type_); 60 | } 61 | 62 | RouteGeneratorPtr RouteGenerator::createGenerator(const RouteType & type) 63 | { 64 | switch (type) { 65 | case RouteType::BOUSTROPHEDON: 66 | return std::move(std::make_shared()); 67 | case RouteType::SNAKE: 68 | return std::move(std::make_shared()); 69 | case RouteType::SPIRAL: 70 | return std::move(std::make_shared()); 71 | case RouteType::CUSTOM: 72 | return std::move(std::make_shared()); 73 | default: 74 | RCLCPP_WARN(logger_, "Unknown route type set!"); 75 | return RouteGeneratorPtr{nullptr}; 76 | } 77 | } 78 | 79 | std::string RouteGenerator::toString(const RouteType & type) 80 | { 81 | switch (type) { 82 | case RouteType::BOUSTROPHEDON: 83 | return "Boustrophedon"; 84 | case RouteType::SNAKE: 85 | return "Snake"; 86 | case RouteType::SPIRAL: 87 | return "Spiral"; 88 | case RouteType::CUSTOM: 89 | return "Custom"; 90 | default: 91 | return "Unknown"; 92 | } 93 | } 94 | 95 | RouteType RouteGenerator::toType(const std::string & str) 96 | { 97 | std::string mode_str = str; 98 | util::toUpper(mode_str); 99 | if (mode_str == "BOUSTROPHEDON") { 100 | return RouteType::BOUSTROPHEDON; 101 | } else if (mode_str == "SNAKE") { 102 | return RouteType::SNAKE; 103 | } else if (mode_str == "SPIRAL") { 104 | return RouteType::SPIRAL; 105 | } else if (mode_str == "CUSTOM") { 106 | return RouteType::CUSTOM; 107 | } else { 108 | return RouteType::UNKNOWN; 109 | } 110 | } 111 | 112 | } // namespace opennav_coverage 113 | -------------------------------------------------------------------------------- /opennav_coverage/src/swath_generator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "opennav_coverage/swath_generator.hpp" 19 | 20 | namespace opennav_coverage 21 | { 22 | 23 | Swaths SwathGenerator::generateSwaths( 24 | const Field & field, const opennav_coverage_msgs::msg::SwathMode & settings) 25 | { 26 | SwathType action_type = toType(settings.objective); 27 | SwathAngleType action_angle_type = toAngleType(settings.mode); 28 | SwathObjectivePtr objective{nullptr}; 29 | float swath_angle = 0.0f; 30 | float step_angle = 0.0f; 31 | 32 | // If not set by action, use default mode 33 | if (action_type == SwathType::UNKNOWN && action_angle_type == SwathAngleType::UNKNOWN) { 34 | action_type = default_type_; 35 | action_angle_type = default_angle_type_; 36 | objective = default_objective_; 37 | swath_angle = default_swath_angle_; 38 | step_angle = default_step_angle_; 39 | } else { 40 | objective = createObjective(action_type); 41 | swath_angle = settings.best_angle; 42 | step_angle = settings.step_angle; 43 | } 44 | 45 | RCLCPP_DEBUG( 46 | logger_, "Generating Swaths with: %s", toString(action_type, action_angle_type).c_str()); 47 | 48 | generator_->setAllowOverlap(default_allow_overlap_); 49 | switch (action_angle_type) { 50 | case SwathAngleType::BRUTE_FORCE: 51 | if (!objective) { 52 | throw CoverageException("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE."); 53 | } 54 | generator_->step_angle = step_angle; 55 | return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field); 56 | case SwathAngleType::SET_ANGLE: 57 | return generator_->generateSwaths(swath_angle, robot_params_->getOperationWidth(), field); 58 | default: 59 | throw CoverageException("No valid swath angle mode set! Options: BRUTE_FORCE, SET_ANGLE."); 60 | } 61 | } 62 | 63 | void SwathGenerator::setSwathMode(const std::string & new_mode) 64 | { 65 | default_type_ = toType(new_mode); 66 | default_objective_ = createObjective(default_type_); 67 | } 68 | 69 | void SwathGenerator::setSwathAngleMode(const std::string & new_mode) 70 | { 71 | default_angle_type_ = toAngleType(new_mode); 72 | } 73 | 74 | SwathObjectivePtr SwathGenerator::createObjective(const SwathType & type) 75 | { 76 | switch (type) { 77 | case SwathType::LENGTH: 78 | return std::move(std::make_shared()); 79 | case SwathType::NUMBER: 80 | return std::move(std::make_shared()); 81 | case SwathType::COVERAGE: 82 | return std::move(std::make_shared()); 83 | default: 84 | RCLCPP_WARN(logger_, "Unknown swath type set!"); 85 | return SwathObjectivePtr{nullptr}; 86 | } 87 | } 88 | 89 | std::string SwathGenerator::toString(const SwathType & type, const SwathAngleType & angle_type) 90 | { 91 | std::string str; 92 | switch (type) { 93 | case SwathType::LENGTH: 94 | str = "Length"; 95 | break; 96 | case SwathType::NUMBER: 97 | str = "Number"; 98 | break; 99 | case SwathType::COVERAGE: 100 | str = "Coverage"; 101 | break; 102 | default: 103 | str = "Unknown"; 104 | break; 105 | } 106 | 107 | str += " Objective and "; 108 | 109 | switch (angle_type) { 110 | case SwathAngleType::SET_ANGLE: 111 | str += "Set Angle"; 112 | break; 113 | case SwathAngleType::BRUTE_FORCE: 114 | str += "Brute Force"; 115 | break; 116 | default: 117 | str += "Unknown"; 118 | break; 119 | } 120 | 121 | str += " angle."; 122 | return str; 123 | } 124 | 125 | SwathType SwathGenerator::toType(const std::string & str) 126 | { 127 | std::string mode_str = str; 128 | util::toUpper(mode_str); 129 | if (mode_str == "LENGTH") { 130 | return SwathType::LENGTH; 131 | } else if (mode_str == "NUMBER") { 132 | return SwathType::NUMBER; 133 | } else if (mode_str == "COVERAGE") { 134 | return SwathType::COVERAGE; 135 | } else { 136 | return SwathType::UNKNOWN; 137 | } 138 | } 139 | 140 | SwathAngleType SwathGenerator::toAngleType(const std::string & str) 141 | { 142 | std::string mode_str = str; 143 | util::toUpper(mode_str); 144 | if (mode_str == "SET_ANGLE") { 145 | return SwathAngleType::SET_ANGLE; 146 | } else if (mode_str == "BRUTE_FORCE") { 147 | return SwathAngleType::BRUTE_FORCE; 148 | } else { 149 | return SwathAngleType::UNKNOWN; 150 | } 151 | } 152 | 153 | 154 | } // namespace opennav_coverage 155 | -------------------------------------------------------------------------------- /opennav_coverage/src/visualizer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "opennav_coverage/visualizer.hpp" 20 | 21 | namespace opennav_coverage 22 | { 23 | 24 | const std::string GLOBAL_FRAME = "map"; // NOLINT 25 | 26 | void Visualizer::deactivate() 27 | { 28 | nav_plan_pub_.reset(); 29 | headlands_pub_.reset(); 30 | planning_field_pub_.reset(); 31 | swaths_pub_.reset(); 32 | } 33 | 34 | void Visualizer::visualize( 35 | const Field & total_field, const Field & no_headland_field, 36 | const Point & ref_pt, const nav_msgs::msg::Path & nav_path, 37 | const Swaths swaths, const std_msgs::msg::Header & header) 38 | { 39 | // F2C strips out reference point of all data, so we need to readd it 40 | // so that our visualizations mirror the true transformed output 41 | 42 | // Visualize coverage path 43 | if (nav_plan_pub_->get_subscription_count() > 0 && nav_path.poses.size() > 0) { 44 | auto utm_path = std::make_unique(nav_path); 45 | utm_path->header.frame_id = GLOBAL_FRAME; 46 | for (unsigned int i = 0; i != utm_path->poses.size(); i++) { 47 | utm_path->poses[i].header.frame_id = GLOBAL_FRAME; 48 | utm_path->poses[i].pose.position.x += ref_pt.getX(); 49 | utm_path->poses[i].pose.position.y += ref_pt.getY(); 50 | } 51 | nav_plan_pub_->publish(std::move(utm_path)); 52 | } 53 | 54 | // Visualize field boundary 55 | if (headlands_pub_->get_subscription_count() > 0) { 56 | auto field_polygon = std::make_unique(); 57 | field_polygon->header.stamp = header.stamp; 58 | field_polygon->header.frame_id = GLOBAL_FRAME; 59 | Polygon boundary = total_field.getGeometry(0); // Only outer-most polygon boundary 60 | for (unsigned int i = 0; i != boundary.size(); i++) { 61 | field_polygon->polygon.points.push_back(util::toMsg(boundary.getGeometry(i) + ref_pt)); 62 | } 63 | headlands_pub_->publish(std::move(field_polygon)); 64 | } 65 | 66 | // Visualize field for planning (after headland removed) 67 | if (planning_field_pub_->get_subscription_count() > 0) { 68 | auto headlandless_polygon = std::make_unique(); 69 | headlandless_polygon->header.stamp = header.stamp; 70 | headlandless_polygon->header.frame_id = GLOBAL_FRAME; 71 | if (no_headland_field.size() > 0) { 72 | Polygon planning_field = no_headland_field.getGeometry(0); // Only outer polygon boundary 73 | for (unsigned int i = 0; i != planning_field.size(); i++) { 74 | headlandless_polygon->polygon.points.push_back( 75 | util::toMsg(planning_field.getGeometry(i) + ref_pt)); 76 | } 77 | planning_field_pub_->publish(std::move(headlandless_polygon)); 78 | } 79 | } 80 | 81 | // Visualize swaths alone 82 | if (swaths_pub_->get_subscription_count() > 0) { 83 | auto output_swaths = std::make_unique(); 84 | output_swaths->header.stamp = header.stamp; 85 | output_swaths->header.frame_id = GLOBAL_FRAME; 86 | output_swaths->action = visualization_msgs::msg::Marker::ADD; 87 | output_swaths->type = visualization_msgs::msg::Marker::LINE_LIST; 88 | output_swaths->pose.orientation.w = 1.0; 89 | output_swaths->scale.x = 0.3; 90 | output_swaths->scale.y = 0.3; 91 | output_swaths->scale.z = 0.3; 92 | output_swaths->color.b = 1.0; 93 | output_swaths->color.a = 1.0; 94 | 95 | for (unsigned int i = 0; i != swaths.size(); i++) { 96 | auto & swath = swaths[i]; 97 | output_swaths->points.push_back( 98 | util::pointToPoint32(util::toMsg(swath.startPoint() + ref_pt))); 99 | output_swaths->points.push_back( 100 | util::pointToPoint32(util::toMsg(swath.endPoint() + ref_pt))); 101 | } 102 | 103 | swaths_pub_->publish(std::move(output_swaths)); 104 | } 105 | } 106 | 107 | } // namespace opennav_coverage 108 | -------------------------------------------------------------------------------- /opennav_coverage/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Test utils 2 | ament_add_gtest(test_utils 3 | test_utils.cpp 4 | ) 5 | ament_target_dependencies(test_utils 6 | ${dependencies} 7 | ) 8 | target_link_libraries(test_utils 9 | ${library_name} 10 | ) 11 | 12 | # Test robot 13 | ament_add_gtest(test_robot 14 | test_robot.cpp 15 | ) 16 | ament_target_dependencies(test_robot 17 | ${dependencies} 18 | ) 19 | target_link_libraries(test_robot 20 | ${library_name} 21 | ) 22 | 23 | # Test visualizations 24 | ament_add_gtest(test_viz 25 | test_viz.cpp 26 | ) 27 | ament_target_dependencies(test_viz 28 | ${dependencies} 29 | ) 30 | target_link_libraries(test_viz 31 | ${library_name} 32 | ) 33 | 34 | # Test headland 35 | ament_add_gtest(test_headland 36 | test_headland.cpp 37 | ) 38 | ament_target_dependencies(test_headland 39 | ${dependencies} 40 | ) 41 | target_link_libraries(test_headland 42 | ${library_name} 43 | ) 44 | 45 | # Test swath 46 | ament_add_gtest(test_swath 47 | test_swath.cpp 48 | ) 49 | ament_target_dependencies(test_swath 50 | ${dependencies} 51 | ) 52 | target_link_libraries(test_swath 53 | ${library_name} 54 | ) 55 | 56 | # Test route 57 | ament_add_gtest(test_route 58 | test_route.cpp 59 | ) 60 | ament_target_dependencies(test_route 61 | ${dependencies} 62 | ) 63 | target_link_libraries(test_route 64 | ${library_name} 65 | ) 66 | 67 | # Test path 68 | ament_add_gtest(test_path 69 | test_path.cpp 70 | ) 71 | ament_target_dependencies(test_path 72 | ${dependencies} 73 | ) 74 | target_link_libraries(test_path 75 | ${library_name} 76 | ) 77 | 78 | # Test server 79 | ament_add_gtest(test_server 80 | test_server.cpp 81 | ) 82 | ament_target_dependencies(test_server 83 | ${dependencies} 84 | ) 85 | target_link_libraries(test_server 86 | ${library_name} 87 | ) 88 | -------------------------------------------------------------------------------- /opennav_coverage/test/cartesian_test_field.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 5.0,5.0 5.0,15.0 15.0,15.0 10.0,5.0 5.0,5.0 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 5.25,5.0 5.5,15.0 19 | 20 | 21 | 22 | 23 | 24 | 25 | 7.7,5.0 6.76,15.0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 8.38,5.0 9.3,15.0 33 | 34 | 35 | 36 | 37 | 38 | 39 | 11.26,7.53 10.37,15.0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 11.84,8.7 11.5,15.0 47 | 48 | 49 | 50 | 51 | 52 | 53 | 13.23,11.47 13.16,15.0 54 | 55 | 56 | 57 | 58 | 59 | 60 | 13.71,12.43 14.15,15.0 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /opennav_coverage/test/irregular_test_field.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 4.261999903178513,51.785970497504699 4.262038589314281,51.786039202423197 4.262347282962439,51.786768209806375 4.262830090865386,51.7878798462305 4.263215324719226,51.788780988539685 4.263449442390776,51.789348181795773 4.256016345252335,51.790638724388899 4.256037421483011,51.790578636884391 4.256620830470679,51.788990944908264 4.257326811374215,51.787053650535569 4.257493994205981,51.786601740034598 4.261951055826343,51.785827833304417 4.261999903178513,51.785970497504699 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 4.26265789653692,51.7874833857072 4.26271354211247,51.7894759738126 18 | 19 | 20 | 21 | 22 | 4.26191592149437,51.7858339346549 4.26202092621845,51.7895962444668 23 | 24 | 25 | 26 | 27 | 4.26122335903712,51.7859542010475 4.26132830657437,51.7897165110625 28 | 29 | 30 | 31 | 32 | 4.26053079283075,51.7860744633821 4.26098199534611,51.7897766428385 33 | 34 | 35 | 36 | 37 | 4.25983822287539,51.7861947216588 4.25994305603656,51.7899570320785 38 | 39 | 40 | 41 | 42 | 4.25914564917117,51.7863149758775 4.25959674105851,51.7900171597959 43 | 44 | 45 | 46 | 47 | 4.25845307171819,51.786435226038 4.25890410829022,51.7901374121867 48 | 49 | 50 | 51 | 52 | 4.2577604905166,51.7865554721404 4.25786515210752,51.790317783163 53 | 54 | 55 | 56 | 57 | 4.25709601998153,51.7876869875219 4.25751883150527,51.7903779047924 58 | 59 | 60 | 61 | 62 | 4.25644928223945,51.7894578087076 4.25647986407474,51.7905582635924 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_field.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 4.26199990317851,51.7859704975047 4.26203858931428,51.7860392024232 4.26234728296244,51.7867682098064 4.26283009086539,51.7878798462305 4.26321532471923,51.7887809885397 4.26344944239078,51.7893481817958 4.25601634525234,51.7906387243889 4.25603742148301,51.7905786368844 4.25662083047068,51.7889909449083 4.25732681137421,51.7870536505356 4.25749399420598,51.7866017400346 4.26195105582634,51.7858278333044 4.26199990317851,51.7859704975047 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 4.26203858931428,51.7860392024232 4.26234728296244,51.7867682098064 4.26283009086539,51.7878798462305 4.26321532471923,51.7887809885397 4.26344944239078,51.7893481817958 19 | 20 | 21 | 22 | 23 | 24 | 25 | 4.26344944239078,51.7893481817958 4.25601634525234,51.7906387243889 26 | 27 | 28 | 29 | 30 | 31 | 32 | 4.25601634525234,51.7906387243889 4.25603742148301,51.7905786368844 33 | 34 | 35 | 36 | 37 | 38 | 39 | 4.25603742148301,51.7905786368844 4.25662083047068,51.7889909449083 4.25732681137421,51.7870536505356 4.25749399420598,51.7866017400346 40 | 41 | 42 | 43 | 44 | 45 | 46 | 4.25749399420598,51.7866017400346 4.26195105582634,51.7858278333044 47 | 48 | 49 | 50 | 51 | 52 | 53 | 4.26195105582634,51.7858278333044 4.26199990317851,51.7859704975047 4.26203858931428,51.7860392024232 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_headland.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "opennav_coverage/headland_generator.hpp" 18 | #include "tf2/utils.h" 19 | #include "fields2cover/utils/random.h" 20 | 21 | // Luckily, F2C has very high test coverage so we only need to test what we touch 22 | 23 | class RosLockGuard 24 | { 25 | public: 26 | RosLockGuard() {rclcpp::init(0, nullptr);} 27 | ~RosLockGuard() {rclcpp::shutdown();} 28 | }; 29 | RosLockGuard g_rclcpp; 30 | 31 | namespace opennav_coverage 32 | { 33 | 34 | class HeadlandShim : public HeadlandGenerator 35 | { 36 | public: 37 | template 38 | explicit HeadlandShim(const NodeT & node) 39 | : HeadlandGenerator(node) 40 | {} 41 | 42 | HeadlandGeneratorPtr createGeneratorShim(const HeadlandType & type) 43 | { 44 | return createGenerator(type); 45 | } 46 | 47 | std::string toStringShim(const HeadlandType & type) 48 | { 49 | return toString(type); 50 | } 51 | 52 | HeadlandType toTypeShim(const std::string & str) 53 | { 54 | return toType(str); 55 | } 56 | }; 57 | 58 | TEST(HeadlandTests, TestheadlandUtils) 59 | { 60 | auto node = std::make_shared("test_node"); 61 | auto generator = HeadlandShim(node); 62 | 63 | EXPECT_EQ(generator.toStringShim(HeadlandType::UNKNOWN), std::string("Unknown")); 64 | EXPECT_EQ(generator.toStringShim(HeadlandType::CONSTANT), std::string("Constant")); 65 | 66 | EXPECT_EQ(generator.toTypeShim("FAKE"), HeadlandType::UNKNOWN); 67 | EXPECT_EQ(generator.toTypeShim("constant"), HeadlandType::CONSTANT); 68 | EXPECT_EQ(generator.toTypeShim("CONSTANT"), HeadlandType::CONSTANT); 69 | 70 | EXPECT_TRUE(generator.createGeneratorShim(HeadlandType::CONSTANT)); 71 | EXPECT_FALSE(generator.createGeneratorShim(HeadlandType::UNKNOWN)); 72 | 73 | generator.setMode("constant"); 74 | } 75 | 76 | TEST(HeadlandTests, TestheadlandGeneration) 77 | { 78 | auto node = std::make_shared("test_node"); 79 | auto generator = HeadlandShim(node); 80 | 81 | // Generate some toy field 82 | f2c::Random rand; 83 | auto field = rand.generateRandField(5, 1e5); 84 | 85 | // Shouldn't throw, results in valid output 86 | opennav_coverage_msgs::msg::HeadlandMode settings; 87 | auto field_out = generator.generateHeadlands(field.field.getGeometry(0), settings); 88 | settings.mode = "CONSTANT"; 89 | auto field_out2 = generator.generateHeadlands(field.field.getGeometry(0), settings); 90 | } 91 | 92 | } // namespace opennav_coverage 93 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_path.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "opennav_coverage/robot_params.hpp" 18 | #include "opennav_coverage/route_generator.hpp" 19 | #include "opennav_coverage/swath_generator.hpp" 20 | #include "opennav_coverage/path_generator.hpp" 21 | #include "tf2/utils.h" 22 | #include "fields2cover/utils/random.h" 23 | 24 | // Luckily, F2C has very high test coverage so we only need to test what we touch 25 | 26 | class RosLockGuard 27 | { 28 | public: 29 | RosLockGuard() {rclcpp::init(0, nullptr);} 30 | ~RosLockGuard() {rclcpp::shutdown();} 31 | }; 32 | RosLockGuard g_rclcpp; 33 | 34 | namespace opennav_coverage 35 | { 36 | 37 | class PathShim : public PathGenerator 38 | { 39 | public: 40 | template 41 | explicit PathShim(const NodeT & node, RobotParams * robot_params) 42 | : PathGenerator(node, robot_params) 43 | {} 44 | 45 | TurningBasePtr createCurveShim(const PathType & type, const PathContinuityType & c_type) 46 | { 47 | return createCurve(type, c_type); 48 | } 49 | 50 | std::string toStringShim(const PathType & type, const PathContinuityType & c_type) 51 | { 52 | return toString(type, c_type); 53 | } 54 | 55 | PathType toTypeShim(const std::string & str) 56 | { 57 | return toType(str); 58 | } 59 | 60 | PathContinuityType toContinuityTypeShim(const std::string & str) 61 | { 62 | return toContinuityType(str); 63 | } 64 | }; 65 | 66 | TEST(PathTests, TestpathUtils) 67 | { 68 | auto node = std::make_shared("test_node"); 69 | RobotParams params(node); 70 | auto generator = PathShim(node, ¶ms); 71 | 72 | EXPECT_EQ(generator.toTypeShim("FAKE"), PathType::UNKNOWN); 73 | EXPECT_EQ(generator.toTypeShim("REEDS_SHEPP"), PathType::REEDS_SHEPP); 74 | EXPECT_EQ(generator.toTypeShim("reeds_shepp"), PathType::REEDS_SHEPP); 75 | EXPECT_EQ(generator.toTypeShim("DUBIN"), PathType::DUBIN); 76 | EXPECT_EQ(generator.toTypeShim("dubin"), PathType::DUBIN); 77 | 78 | EXPECT_EQ(generator.toContinuityTypeShim("FAKE"), PathContinuityType::UNKNOWN); 79 | EXPECT_EQ(generator.toContinuityTypeShim("CONTINUOUS"), PathContinuityType::CONTINUOUS); 80 | EXPECT_EQ(generator.toContinuityTypeShim("continuous"), PathContinuityType::CONTINUOUS); 81 | EXPECT_EQ(generator.toContinuityTypeShim("DISCONTINUOUS"), PathContinuityType::DISCONTINUOUS); 82 | EXPECT_EQ(generator.toContinuityTypeShim("discontinuous"), PathContinuityType::DISCONTINUOUS); 83 | 84 | EXPECT_GT(generator.toStringShim(PathType::UNKNOWN, PathContinuityType::UNKNOWN).size(), 20u); 85 | EXPECT_GT( 86 | generator.toStringShim( 87 | PathType::REEDS_SHEPP, PathContinuityType::CONTINUOUS).size(), 20u); 88 | EXPECT_GT( 89 | generator.toStringShim( 90 | PathType::DUBIN, PathContinuityType::DISCONTINUOUS).size(), 20u); 91 | 92 | EXPECT_TRUE(generator.createCurveShim(PathType::REEDS_SHEPP, PathContinuityType::CONTINUOUS)); 93 | EXPECT_TRUE(generator.createCurveShim(PathType::REEDS_SHEPP, PathContinuityType::DISCONTINUOUS)); 94 | EXPECT_TRUE(generator.createCurveShim(PathType::DUBIN, PathContinuityType::CONTINUOUS)); 95 | EXPECT_TRUE(generator.createCurveShim(PathType::DUBIN, PathContinuityType::DISCONTINUOUS)); 96 | EXPECT_FALSE(generator.createCurveShim(PathType::UNKNOWN, PathContinuityType::UNKNOWN)); 97 | EXPECT_FALSE(generator.createCurveShim(PathType::UNKNOWN, PathContinuityType::DISCONTINUOUS)); 98 | EXPECT_FALSE(generator.createCurveShim(PathType::UNKNOWN, PathContinuityType::CONTINUOUS)); 99 | EXPECT_FALSE(generator.createCurveShim(PathType::REEDS_SHEPP, PathContinuityType::UNKNOWN)); 100 | EXPECT_FALSE(generator.createCurveShim(PathType::DUBIN, PathContinuityType::UNKNOWN)); 101 | 102 | generator.setPathMode("a mode"); 103 | generator.setPathContinuityMode("another mode"); 104 | generator.setTurnPointDistance(0.1); 105 | } 106 | 107 | TEST(PathTests, TestpathGeneration) 108 | { 109 | auto node = std::make_shared("test_node"); 110 | RobotParams robot_params(node); 111 | SwathGenerator swath_gen(node, &robot_params); 112 | RouteGenerator route_gen(node); 113 | PathShim generator(node, &robot_params); 114 | 115 | // Generate some toy route 116 | f2c::Random rand; 117 | auto field = rand.generateRandField(5, 1e5); 118 | opennav_coverage_msgs::msg::SwathMode sw_settings; 119 | auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings); 120 | opennav_coverage_msgs::msg::RouteMode rt_settings; 121 | auto route = route_gen.generateRoute(swaths, rt_settings); 122 | 123 | // Shouldn't throw, results in valid output 124 | opennav_coverage_msgs::msg::PathMode settings; 125 | auto path1 = generator.generatePath(route, settings); 126 | settings.mode = "REEDS_SHEPP"; 127 | settings.continuity_mode = "CONTINUOUS"; 128 | auto path2 = generator.generatePath(route, settings); 129 | } 130 | 131 | } // namespace opennav_coverage 132 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_robot.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "opennav_coverage/robot_params.hpp" 18 | #include "tf2/utils.h" 19 | 20 | // Luckily, F2C has very high test coverage so we only need to test what we touch 21 | 22 | class RosLockGuard 23 | { 24 | public: 25 | RosLockGuard() {rclcpp::init(0, nullptr);} 26 | ~RosLockGuard() {rclcpp::shutdown();} 27 | }; 28 | RosLockGuard g_rclcpp; 29 | 30 | namespace opennav_coverage 31 | { 32 | 33 | TEST(RobotTests, Testrobot) 34 | { 35 | auto node = std::make_shared("test_node"); 36 | RobotParams robot(node); 37 | 38 | EXPECT_EQ(robot.getWidth(), 2.1); 39 | EXPECT_EQ(robot.getOperationWidth(), 2.5); 40 | EXPECT_EQ(robot.getRobot().linear_curv_change, 2.0); 41 | } 42 | 43 | } // namespace opennav_coverage 44 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_route.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "opennav_coverage/robot_params.hpp" 18 | #include "opennav_coverage/route_generator.hpp" 19 | #include "opennav_coverage/swath_generator.hpp" 20 | #include "tf2/utils.h" 21 | #include "fields2cover/utils/random.h" 22 | 23 | // Luckily, F2C has very high test coverage so we only need to test what we touch 24 | 25 | class RosLockGuard 26 | { 27 | public: 28 | RosLockGuard() {rclcpp::init(0, nullptr);} 29 | ~RosLockGuard() {rclcpp::shutdown();} 30 | }; 31 | RosLockGuard g_rclcpp; 32 | 33 | namespace opennav_coverage 34 | { 35 | 36 | class RouteShim : public RouteGenerator 37 | { 38 | public: 39 | template 40 | explicit RouteShim(const NodeT & node) 41 | : RouteGenerator(node) 42 | {} 43 | 44 | RouteGeneratorPtr createGeneratorShim(const RouteType & type) 45 | { 46 | return createGenerator(type); 47 | } 48 | 49 | std::string toStringShim(const RouteType & type) 50 | { 51 | return toString(type); 52 | } 53 | 54 | RouteType toTypeShim(const std::string & str) 55 | { 56 | return toType(str); 57 | } 58 | }; 59 | 60 | TEST(RouteTests, TestrouteUtils) 61 | { 62 | auto node = std::make_shared("test_node"); 63 | auto generator = RouteShim(node); 64 | 65 | EXPECT_EQ(generator.toTypeShim("FAKE"), RouteType::UNKNOWN); 66 | EXPECT_EQ(generator.toTypeShim("BOUSTROPHEDON"), RouteType::BOUSTROPHEDON); 67 | EXPECT_EQ(generator.toTypeShim("boustrophedon"), RouteType::BOUSTROPHEDON); 68 | EXPECT_EQ(generator.toTypeShim("SNAKE"), RouteType::SNAKE); 69 | EXPECT_EQ(generator.toTypeShim("snake"), RouteType::SNAKE); 70 | EXPECT_EQ(generator.toTypeShim("SPIRAL"), RouteType::SPIRAL); 71 | EXPECT_EQ(generator.toTypeShim("spiral"), RouteType::SPIRAL); 72 | EXPECT_EQ(generator.toTypeShim("CUSTOM"), RouteType::CUSTOM); 73 | EXPECT_EQ(generator.toTypeShim("custom"), RouteType::CUSTOM); 74 | 75 | EXPECT_EQ(generator.toStringShim(RouteType::UNKNOWN), std::string("Unknown")); 76 | EXPECT_EQ(generator.toStringShim(RouteType::BOUSTROPHEDON), std::string("Boustrophedon")); 77 | EXPECT_EQ(generator.toStringShim(RouteType::SNAKE), std::string("Snake")); 78 | EXPECT_EQ(generator.toStringShim(RouteType::SPIRAL), std::string("Spiral")); 79 | EXPECT_EQ(generator.toStringShim(RouteType::CUSTOM), std::string("Custom")); 80 | 81 | EXPECT_TRUE(generator.createGeneratorShim(RouteType::BOUSTROPHEDON)); 82 | EXPECT_TRUE(generator.createGeneratorShim(RouteType::SNAKE)); 83 | EXPECT_TRUE(generator.createGeneratorShim(RouteType::SPIRAL)); 84 | EXPECT_TRUE(generator.createGeneratorShim(RouteType::CUSTOM)); 85 | EXPECT_FALSE(generator.createGeneratorShim(RouteType::UNKNOWN)); 86 | 87 | generator.setMode("a mode"); 88 | generator.setSpiralN(10); 89 | generator.setCustomOrder(std::vector{}); // NOLINT 90 | } 91 | 92 | TEST(RouteTests, TestrouteGeneration) 93 | { 94 | auto node = std::make_shared("test_node"); 95 | RobotParams robot_params(node); 96 | SwathGenerator swath_gen(node, &robot_params); 97 | opennav_coverage_msgs::msg::SwathMode sw_settings; 98 | RouteShim generator(node); 99 | 100 | // Generate some toy field 101 | f2c::Random rand; 102 | auto field = rand.generateRandField(5, 1e5); 103 | auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings); 104 | 105 | // Shouldn't throw, results in valid output 106 | opennav_coverage_msgs::msg::RouteMode settings; 107 | auto route1 = generator.generateRoute(swaths, settings); 108 | settings.mode = "BOUSTROPHEDON"; 109 | auto route2 = generator.generateRoute(swaths, settings); 110 | settings.mode = "SPIRAL"; 111 | auto route3 = generator.generateRoute(swaths, settings); 112 | 113 | // Throws since custom order is set to emptry set 114 | settings.mode = "CUSTOM"; 115 | EXPECT_THROW(generator.generateRoute(swaths, settings), std::length_error); 116 | } 117 | 118 | } // namespace opennav_coverage 119 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_swath.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "opennav_coverage/robot_params.hpp" 18 | #include "opennav_coverage/swath_generator.hpp" 19 | #include "tf2/utils.h" 20 | #include "fields2cover/utils/random.h" 21 | 22 | // Luckily, F2C has very high test coverage so we only need to test what we touch 23 | 24 | class RosLockGuard 25 | { 26 | public: 27 | RosLockGuard() {rclcpp::init(0, nullptr);} 28 | ~RosLockGuard() {rclcpp::shutdown();} 29 | }; 30 | RosLockGuard g_rclcpp; 31 | 32 | namespace opennav_coverage 33 | { 34 | 35 | class SwathShim : public SwathGenerator 36 | { 37 | public: 38 | template 39 | explicit SwathShim(const NodeT & node, RobotParams * robot_params) 40 | : SwathGenerator(node, robot_params) 41 | {} 42 | 43 | SwathObjectivePtr createObjectiveShim(const SwathType & type) 44 | { 45 | return createObjective(type); 46 | } 47 | 48 | std::string toStringShim(const SwathType & type, const SwathAngleType & angle_type) 49 | { 50 | return toString(type, angle_type); 51 | } 52 | 53 | SwathType toTypeShim(const std::string & str) 54 | { 55 | return toType(str); 56 | } 57 | 58 | 59 | SwathAngleType toAngleTypeShim(const std::string & str) 60 | { 61 | return toAngleType(str); 62 | } 63 | }; 64 | 65 | TEST(SwathTests, TestswathUtils) 66 | { 67 | auto node = std::make_shared("test_node"); 68 | RobotParams robot(node); 69 | auto generator = SwathShim(node, &robot); 70 | 71 | EXPECT_EQ(generator.toAngleTypeShim("FAKE"), SwathAngleType::UNKNOWN); 72 | EXPECT_EQ(generator.toAngleTypeShim("SET_ANGLE"), SwathAngleType::SET_ANGLE); 73 | EXPECT_EQ(generator.toAngleTypeShim("set_angle"), SwathAngleType::SET_ANGLE); 74 | EXPECT_EQ(generator.toAngleTypeShim("BRUTE_FORCE"), SwathAngleType::BRUTE_FORCE); 75 | EXPECT_EQ(generator.toAngleTypeShim("brute_force"), SwathAngleType::BRUTE_FORCE); 76 | 77 | EXPECT_EQ(generator.toTypeShim("FAKE"), SwathType::UNKNOWN); 78 | EXPECT_EQ(generator.toTypeShim("LENGTH"), SwathType::LENGTH); 79 | EXPECT_EQ(generator.toTypeShim("length"), SwathType::LENGTH); 80 | EXPECT_EQ(generator.toTypeShim("NUMBER"), SwathType::NUMBER); 81 | EXPECT_EQ(generator.toTypeShim("number"), SwathType::NUMBER); 82 | EXPECT_EQ(generator.toTypeShim("COVERAGE"), SwathType::COVERAGE); 83 | EXPECT_EQ(generator.toTypeShim("coverage"), SwathType::COVERAGE); 84 | 85 | 86 | EXPECT_EQ(generator.toStringShim(SwathType::NUMBER, SwathAngleType::SET_ANGLE).size(), 37u); 87 | EXPECT_EQ(generator.toStringShim(SwathType::COVERAGE, SwathAngleType::BRUTE_FORCE).size(), 41u); 88 | EXPECT_GT(generator.toStringShim(SwathType::UNKNOWN, SwathAngleType::UNKNOWN).size(), 20u); 89 | EXPECT_GT(generator.toStringShim(SwathType::UNKNOWN, SwathAngleType::BRUTE_FORCE).size(), 20u); 90 | 91 | EXPECT_TRUE(generator.createObjectiveShim(SwathType::LENGTH)); 92 | EXPECT_TRUE(generator.createObjectiveShim(SwathType::NUMBER)); 93 | EXPECT_TRUE(generator.createObjectiveShim(SwathType::COVERAGE)); 94 | EXPECT_FALSE(generator.createObjectiveShim(SwathType::UNKNOWN)); 95 | 96 | generator.setSwathAngleMode("NUMBER"); 97 | generator.setSwathMode("SET_ANGLE"); 98 | generator.setSwathAngle(0.0); 99 | generator.setOVerlap(false); 100 | generator.setStepAngle(false); 101 | } 102 | 103 | TEST(SwathTests, TestswathGeneration) 104 | { 105 | auto node = std::make_shared("test_node"); 106 | RobotParams robot(node); 107 | auto generator = SwathShim(node, &robot); 108 | 109 | // Generate some toy field 110 | f2c::Random rand; 111 | auto field = rand.generateRandField(5, 1e5); 112 | 113 | // Shouldn't throw, results in valid output 114 | opennav_coverage_msgs::msg::SwathMode settings; 115 | auto swaths1 = generator.generateSwaths(field.field.getGeometry(0), settings); 116 | settings.mode = "BRUTE_FORCE"; 117 | settings.objective = "LENGTH"; 118 | auto swaths2 = generator.generateSwaths(field.field.getGeometry(0), settings); 119 | settings.mode = "SET_ANGLE"; 120 | settings.objective = "NUMBER"; 121 | auto swaths3 = generator.generateSwaths(field.field.getGeometry(0), settings); 122 | } 123 | 124 | } // namespace opennav_coverage 125 | -------------------------------------------------------------------------------- /opennav_coverage/test/test_viz.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "opennav_coverage/visualizer.hpp" 18 | #include "tf2/utils.h" 19 | 20 | // Luckily, F2C has very high test coverage so we only need to test what we touch 21 | 22 | class RosLockGuard 23 | { 24 | public: 25 | RosLockGuard() {rclcpp::init(0, nullptr);} 26 | ~RosLockGuard() {rclcpp::shutdown();} 27 | }; 28 | RosLockGuard g_rclcpp; 29 | 30 | namespace opennav_coverage 31 | { 32 | 33 | TEST(VizTests, Testlifecycle) 34 | { 35 | auto node = std::make_shared("test_node"); 36 | Visualizer viz; 37 | viz.activate(node); 38 | viz.deactivate(); 39 | } 40 | 41 | TEST(VizTests, TestVizPubs) 42 | { 43 | auto node = std::make_shared("test_node"); 44 | rclcpp::Rate r(2.0); 45 | bool got_path_msg = false; 46 | bool got_headland_msg = false; 47 | bool got_planning_field_msg = false; 48 | bool got_swaths_msg = false; 49 | 50 | auto path_sub = node->create_subscription( 51 | "coverage_server/coverage_plan", rclcpp::QoS(1), [&]( 52 | const nav_msgs::msg::Path::SharedPtr /*msg*/) { 53 | got_path_msg = true; 54 | }); 55 | 56 | auto headland_sub = node->create_subscription( 57 | "coverage_server/field_boundary", rclcpp::QoS(1), [&]( 58 | const geometry_msgs::msg::PolygonStamped::SharedPtr /*msg*/) { 59 | got_headland_msg = true; 60 | }); 61 | 62 | auto planning_field_sub = node->create_subscription( 63 | "coverage_server/planning_field", rclcpp::QoS(1), [&]( 64 | const geometry_msgs::msg::PolygonStamped::SharedPtr /*msg*/) { 65 | got_planning_field_msg = true; 66 | }); 67 | 68 | auto swaths_sub = node->create_subscription( 69 | "coverage_server/swaths", rclcpp::QoS(1), [&]( 70 | const visualization_msgs::msg::Marker::SharedPtr /*msg*/) { 71 | got_swaths_msg = true; 72 | }); 73 | 74 | // give a moment for everything to register 75 | r.sleep(); 76 | 77 | Visualizer viz; 78 | viz.activate(node); 79 | 80 | Polygon outer_polygon; 81 | outer_polygon.addPoint(Point(0.0, 1.0)); 82 | outer_polygon.addPoint(Point(1.0, 1.0)); 83 | outer_polygon.addPoint(Point(1.0, 1.0)); 84 | outer_polygon.addPoint(Point(0.0, 1.0)); 85 | Field total_field(outer_polygon); 86 | 87 | std_msgs::msg::Header header; 88 | const auto result = std::make_shared(); 89 | result->nav_path.poses.resize(20); 90 | result->coverage_path.swaths.resize(20); 91 | Field no_headland_field = total_field.clone(); 92 | std::vector swaths_raw; 93 | swaths_raw.resize(20); 94 | Swaths swaths(swaths_raw); 95 | nav_msgs::msg::Path path; 96 | viz.visualize(total_field, no_headland_field, Point(), path, swaths, header); 97 | 98 | // Give a moment to process for stability, then check if received 99 | rclcpp::spin_some(node); 100 | r.sleep(); 101 | EXPECT_FALSE(got_path_msg); // Path empty, unpublished 102 | EXPECT_TRUE(got_headland_msg); 103 | EXPECT_TRUE(got_planning_field_msg); 104 | EXPECT_TRUE(got_swaths_msg); 105 | } 106 | 107 | } // namespace opennav_coverage 108 | -------------------------------------------------------------------------------- /opennav_coverage_bt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(opennav_coverage_bt) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(rclcpp_action REQUIRED) 7 | find_package(std_msgs REQUIRED) 8 | find_package(nav2_util REQUIRED) 9 | find_package(nav2_msgs REQUIRED) 10 | find_package(nav2_core REQUIRED) 11 | find_package(nav2_behavior_tree REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(opennav_coverage_msgs REQUIRED) 15 | find_package(behaviortree_cpp REQUIRED) 16 | 17 | # potentially replace with nav2_common, nav2_package() 18 | set(CMAKE_CXX_STANDARD 17) 19 | add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) 20 | 21 | include_directories( 22 | include 23 | ) 24 | 25 | set(dependencies 26 | rclcpp 27 | rclcpp_action 28 | std_msgs 29 | nav2_util 30 | nav2_msgs 31 | nav2_core 32 | nav2_behavior_tree 33 | nav_msgs 34 | geometry_msgs 35 | opennav_coverage_msgs 36 | behaviortree_cpp 37 | ) 38 | 39 | add_library(opennav_compute_complete_coverage_action_bt_node SHARED src/compute_complete_coverage_path.cpp) 40 | list(APPEND plugin_libs opennav_compute_complete_coverage_action_bt_node) 41 | add_library(opennav_cancel_complete_coverage_action_bt_node SHARED src/cancel_complete_coverage_path.cpp) 42 | list(APPEND plugin_libs opennav_cancel_complete_coverage_action_bt_node) 43 | 44 | foreach(bt_plugin ${plugin_libs}) 45 | ament_target_dependencies(${bt_plugin} ${dependencies}) 46 | target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) 47 | endforeach() 48 | 49 | install(TARGETS ${plugin_libs} 50 | ARCHIVE DESTINATION lib 51 | LIBRARY DESTINATION lib 52 | RUNTIME DESTINATION bin 53 | ) 54 | 55 | install(DIRECTORY include/ 56 | DESTINATION include/ 57 | ) 58 | 59 | install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) 60 | 61 | if(BUILD_TESTING) 62 | find_package(ament_lint_auto REQUIRED) 63 | find_package(ament_cmake_gtest REQUIRED) 64 | ament_lint_auto_find_test_dependencies() 65 | add_subdirectory(test) 66 | endif() 67 | 68 | ament_export_include_directories(include) 69 | ament_export_libraries(${plugin_libs}) 70 | ament_export_dependencies(${dependencies}) 71 | ament_package() 72 | -------------------------------------------------------------------------------- /opennav_coverage_bt/README.md: -------------------------------------------------------------------------------- 1 | # Open Navigation's Nav2 Complete Coverage BT 2 | 3 | This package contains a set of behavior tree nodes, XML for interacting with the coverage servers 4 | -------------------------------------------------------------------------------- /opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml: -------------------------------------------------------------------------------- 1 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml: -------------------------------------------------------------------------------- 1 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml: -------------------------------------------------------------------------------- 1 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /opennav_coverage_bt/include/opennav_coverage_bt/cancel_complete_coverage_path.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE_BT__CANCEL_COMPLETE_COVERAGE_PATH_HPP_ 16 | #define OPENNAV_COVERAGE_BT__CANCEL_COMPLETE_COVERAGE_PATH_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "opennav_coverage_msgs/action/compute_coverage_path.hpp" 22 | #include "nav2_behavior_tree/bt_cancel_action_node.hpp" 23 | #include "opennav_coverage_bt/utils.hpp" 24 | 25 | namespace opennav_coverage_bt 26 | { 27 | 28 | /** 29 | * @brief A nav2_behavior_tree::BtActionNode class that wraps 30 | * opennav_coverage_msgs::action::ComputeCoveragePath 31 | */ 32 | class CoverageCancel 33 | : public nav2_behavior_tree::BtCancelActionNode< 34 | opennav_coverage_msgs::action::ComputeCoveragePath> 35 | { 36 | public: 37 | /** 38 | * @brief A constructor for nav2_behavior_tree::CoverageCancel 39 | * @param xml_tag_name Name for the XML tag for this node 40 | * @param action_name Action name this node creates a client for 41 | * @param conf BT node configuration 42 | */ 43 | CoverageCancel( 44 | const std::string & xml_tag_name, 45 | const std::string & action_name, 46 | const BT::NodeConfiguration & conf); 47 | 48 | /** 49 | * @brief Creates list of BT ports 50 | * @return BT::PortsList Containing basic ports along with node-specific ports 51 | */ 52 | static BT::PortsList providedPorts() 53 | { 54 | return providedBasicPorts( 55 | { 56 | }); 57 | } 58 | }; 59 | 60 | } // namespace opennav_coverage_bt 61 | 62 | #endif // OPENNAV_COVERAGE_BT__CANCEL_COMPLETE_COVERAGE_PATH_HPP_ 63 | -------------------------------------------------------------------------------- /opennav_coverage_bt/include/opennav_coverage_bt/compute_complete_coverage_path.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE_BT__COMPUTE_COMPLETE_COVERAGE_PATH_HPP_ 16 | #define OPENNAV_COVERAGE_BT__COMPUTE_COMPLETE_COVERAGE_PATH_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "opennav_coverage_msgs/action/compute_coverage_path.hpp" 23 | #include "nav2_behavior_tree/bt_action_node.hpp" 24 | #include "geometry_msgs/msg/polygon.hpp" 25 | #include "opennav_coverage_bt/utils.hpp" 26 | 27 | namespace opennav_coverage_bt 28 | { 29 | 30 | /** 31 | * @brief nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputeCoveragePath 32 | */ 33 | class ComputeCoveragePathAction 34 | : public nav2_behavior_tree::BtActionNode< 35 | opennav_coverage_msgs::action::ComputeCoveragePath> 36 | { 37 | using Action = opennav_coverage_msgs::action::ComputeCoveragePath; 38 | using ActionResult = Action::Result; 39 | 40 | public: 41 | /** 42 | * @brief A constructor for opennav_coverage_bt::ComputeCoveragePathAction 43 | * @param xml_tag_name Name for the XML tag for this node 44 | * @param action_name Action name this node creates a client for 45 | * @param conf BT node configuration 46 | */ 47 | ComputeCoveragePathAction( 48 | const std::string & xml_tag_name, 49 | const std::string & action_name, 50 | const BT::NodeConfiguration & conf); 51 | 52 | /** 53 | * @brief Function to perform some user-defined operation on tick 54 | */ 55 | void on_tick() override; 56 | 57 | /** 58 | * @brief Function to perform some user-defined operation upon successful completion of the action 59 | */ 60 | BT::NodeStatus on_success() override; 61 | 62 | /** 63 | * @brief Function to perform some user-defined operation upon abortion of the action 64 | */ 65 | BT::NodeStatus on_aborted() override; 66 | 67 | /** 68 | * @brief Function to perform some user-defined operation upon cancellation of the action 69 | */ 70 | BT::NodeStatus on_cancelled() override; 71 | 72 | /** 73 | * \brief Override required by the a BT action. Cancel the action and set the path output 74 | */ 75 | void halt() override; 76 | 77 | /** 78 | * @brief Creates list of BT ports 79 | * @return BT::PortsList Containing basic ports along with node-specific ports 80 | */ 81 | static BT::PortsList providedPorts() 82 | { 83 | return providedBasicPorts( 84 | { 85 | BT::InputPort("generate_headland", true, "Whether to generate headland"), 86 | BT::InputPort("generate_route", true, "Whether to ordered route of swaths"), 87 | BT::InputPort("generate_path", true, "Whether to generate connected path of routes"), 88 | 89 | BT::InputPort("file_field", "Filepath to field GML file"), 90 | BT::InputPort("file_field_id", 0, "Ordered ID of which field to use in GML File"), 91 | BT::InputPort>( 92 | "polygons", "Port-provided polygon, if not from file"), 93 | BT::InputPort("polygons_frame_id", "map", "Port-provided polygon's frame"), 94 | 95 | BT::OutputPort( 96 | "error_code_id", "The complete coverage error code"), 97 | BT::OutputPort( 98 | "planning_time", "The time to compute coverage plan"), 99 | BT::OutputPort( 100 | "nav_path", "The coverage plan as a nav_msgs/Path to track directly"), 101 | BT::OutputPort( 102 | "coverage_path", "The coverage plan as an ordered set of swaths and route connections"), 103 | }); 104 | } 105 | }; 106 | 107 | } // namespace opennav_coverage_bt 108 | 109 | #endif // OPENNAV_COVERAGE_BT__COMPUTE_COMPLETE_COVERAGE_PATH_HPP_ 110 | -------------------------------------------------------------------------------- /opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE_BT__UTILS_HPP_ 16 | #define OPENNAV_COVERAGE_BT__UTILS_HPP_ 17 | 18 | #include 19 | #include "behaviortree_cpp/behavior_tree.h" 20 | 21 | namespace BT 22 | { 23 | 24 | /** 25 | * @brief Specialization for unsigned short (uint16) 26 | * @return unsigned short from BT port 27 | */ 28 | template<> 29 | inline unsigned short convertFromString(const StringView str) // NOLINT 30 | { 31 | unsigned short result = 0; // NOLINT 32 | auto [ptr, ec] = std::from_chars(str.data(), str.data() + str.size(), result); 33 | if (ec != std::errc()) { 34 | throw RuntimeError(StrCat("Can't convert string [", str, "] to unsigned short")); 35 | } 36 | return result; 37 | } 38 | 39 | } // namespace BT 40 | 41 | #endif // OPENNAV_COVERAGE_BT__UTILS_HPP_ 42 | -------------------------------------------------------------------------------- /opennav_coverage_bt/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opennav_coverage_bt 5 | 0.0.1 6 | A set of BT nodes and XMLs for complete coverage planning 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | nav2_behavior_tree 15 | nav2_util 16 | nav2_core 17 | nav2_msgs 18 | nav_msgs 19 | geometry_msgs 20 | opennav_coverage_msgs 21 | behaviortree_cpp 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /opennav_coverage_bt/src/cancel_complete_coverage_path.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "opennav_coverage_bt/cancel_complete_coverage_path.hpp" 19 | 20 | namespace opennav_coverage_bt 21 | { 22 | 23 | CoverageCancel::CoverageCancel( 24 | const std::string & xml_tag_name, 25 | const std::string & action_name, 26 | const BT::NodeConfiguration & conf) 27 | : BtCancelActionNode( 28 | xml_tag_name, action_name, conf) 29 | { 30 | } 31 | 32 | } // namespace opennav_coverage_bt 33 | 34 | #include "behaviortree_cpp/bt_factory.h" 35 | BT_REGISTER_NODES(factory) 36 | { 37 | BT::NodeBuilder builder = 38 | [](const std::string & name, const BT::NodeConfiguration & config) 39 | { 40 | return std::make_unique( 41 | name, "compute_coverage_path", config); 42 | }; 43 | 44 | factory.registerBuilder( 45 | "CancelCoverage", builder); 46 | } 47 | -------------------------------------------------------------------------------- /opennav_coverage_bt/src/compute_complete_coverage_path.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "opennav_coverage_bt/compute_complete_coverage_path.hpp" 19 | 20 | namespace opennav_coverage_bt 21 | { 22 | 23 | ComputeCoveragePathAction::ComputeCoveragePathAction( 24 | const std::string & xml_tag_name, 25 | const std::string & action_name, 26 | const BT::NodeConfiguration & conf) 27 | : BtActionNode(xml_tag_name, action_name, conf) 28 | { 29 | } 30 | 31 | void ComputeCoveragePathAction::on_tick() 32 | { 33 | // Get core inputs about what to perform 34 | getInput("generate_headland", goal_.generate_headland); 35 | getInput("generate_route", goal_.generate_route); 36 | getInput("generate_path", goal_.generate_path); 37 | 38 | // Get the field to get coverage for 39 | std::string gml_filename; 40 | if (getInput("file_field", gml_filename)) { 41 | goal_.gml_field = gml_filename; 42 | goal_.use_gml_file = true; 43 | } else { 44 | getInput("polygons_frame_id", goal_.frame_id); 45 | 46 | // Convert from vector of Polygons to coverage sp. message 47 | std::vector polys; 48 | getInput("polygons", polys); 49 | goal_.polygons.clear(); 50 | goal_.polygons.resize(polys.size()); 51 | for (unsigned int i = 0; i != polys.size(); i++) { 52 | for (unsigned int j = 0; j != polys[i].points.size(); j++) { 53 | opennav_coverage_msgs::msg::Coordinate coord; 54 | coord.axis1 = polys[i].points[j].x; 55 | coord.axis2 = polys[i].points[j].y; 56 | goal_.polygons[i].coordinates.push_back(coord); 57 | } 58 | } 59 | } 60 | } 61 | 62 | BT::NodeStatus ComputeCoveragePathAction::on_success() 63 | { 64 | setOutput("planning_time", result_.result->planning_time); 65 | setOutput("nav_path", result_.result->nav_path); 66 | setOutput("coverage_path", result_.result->coverage_path); 67 | setOutput("error_code_id", ActionResult::NONE); 68 | return BT::NodeStatus::SUCCESS; 69 | } 70 | 71 | BT::NodeStatus ComputeCoveragePathAction::on_aborted() 72 | { 73 | nav_msgs::msg::Path empty_path; 74 | opennav_coverage_msgs::msg::PathComponents cov_path; 75 | setOutput("nav_path", empty_path); 76 | setOutput("coverage_path", cov_path); 77 | setOutput("error_code_id", result_.result->error_code); 78 | return BT::NodeStatus::FAILURE; 79 | } 80 | 81 | BT::NodeStatus ComputeCoveragePathAction::on_cancelled() 82 | { 83 | nav_msgs::msg::Path empty_path; 84 | opennav_coverage_msgs::msg::PathComponents cov_path; 85 | setOutput("nav_path", empty_path); 86 | setOutput("coverage_path", cov_path); 87 | setOutput("error_code_id", ActionResult::NONE); 88 | return BT::NodeStatus::SUCCESS; 89 | } 90 | 91 | void ComputeCoveragePathAction::halt() 92 | { 93 | nav_msgs::msg::Path empty_path; 94 | opennav_coverage_msgs::msg::PathComponents cov_path; 95 | setOutput("nav_path", empty_path); 96 | setOutput("coverage_path", cov_path); 97 | BtActionNode::halt(); 98 | } 99 | 100 | } // namespace opennav_coverage_bt 101 | 102 | #include "behaviortree_cpp/bt_factory.h" 103 | BT_REGISTER_NODES(factory) 104 | { 105 | BT::NodeBuilder builder = 106 | [](const std::string & name, const BT::NodeConfiguration & config) 107 | { 108 | return std::make_unique( 109 | name, "compute_coverage_path", config); 110 | }; 111 | 112 | factory.registerBuilder( 113 | "ComputeCoveragePath", builder); 114 | } 115 | -------------------------------------------------------------------------------- /opennav_coverage_bt/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Cancel test 2 | ament_add_gtest(test_cancel_complete_coverage test_cancel_complete_coverage.cpp) 3 | target_link_libraries(test_cancel_complete_coverage opennav_cancel_complete_coverage_action_bt_node) 4 | ament_target_dependencies(test_cancel_complete_coverage ${dependencies}) 5 | 6 | # Make command test 7 | ament_add_gtest(test_compute_coverage_path test_compute_coverage_path.cpp) 8 | target_link_libraries(test_compute_coverage_path opennav_compute_complete_coverage_action_bt_node) 9 | ament_target_dependencies(test_compute_coverage_path ${dependencies}) 10 | -------------------------------------------------------------------------------- /opennav_coverage_bt/test/test_compute_coverage_path.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "nav_msgs/msg/path.hpp" 21 | #include "geometry_msgs/msg/pose_stamped.hpp" 22 | 23 | #include "behaviortree_cpp/bt_factory.h" 24 | 25 | #include "nav2_behavior_tree/utils/test_action_server.hpp" 26 | #include "opennav_coverage_bt/compute_complete_coverage_path.hpp" 27 | 28 | class ComputeCompleteCoveragePathActionServer 29 | : public TestActionServer 30 | { 31 | public: 32 | ComputeCompleteCoveragePathActionServer() 33 | : TestActionServer("compute_coverage_path") 34 | {} 35 | 36 | protected: 37 | void execute( 38 | const typename std::shared_ptr< 39 | rclcpp_action::ServerGoalHandle> 40 | goal_handle) 41 | override 42 | { 43 | const auto goal = goal_handle->get_goal(); 44 | auto result = 45 | std::make_shared(); 46 | result->nav_path.poses.resize(2); 47 | result->nav_path.poses[1].pose.position.x = 1.0; 48 | result->nav_path.poses[0].pose.position.x = 0.0; 49 | goal_handle->succeed(result); 50 | } 51 | }; 52 | 53 | class ComputeCoveragePathActionTestFixture : public ::testing::Test 54 | { 55 | public: 56 | static void SetUpTestCase() 57 | { 58 | node_ = std::make_shared("compute_coverage_path_action_test_fixture"); 59 | factory_ = std::make_shared(); 60 | 61 | config_ = new BT::NodeConfiguration(); 62 | 63 | // Create the blackboard that will be shared by all of the nodes in the tree 64 | config_->blackboard = BT::Blackboard::create(); 65 | // Put items on the blackboard 66 | config_->blackboard->set( 67 | "node", 68 | node_); 69 | config_->blackboard->set( 70 | "server_timeout", 71 | std::chrono::milliseconds(20)); 72 | config_->blackboard->set( 73 | "bt_loop_duration", 74 | std::chrono::milliseconds(10)); 75 | config_->blackboard->set( 76 | "wait_for_service_timeout", 77 | std::chrono::milliseconds(1000)); 78 | config_->blackboard->set("initial_pose_received", false); 79 | 80 | BT::NodeBuilder builder = 81 | [](const std::string & name, const BT::NodeConfiguration & config) 82 | { 83 | return std::make_unique( 84 | name, "compute_coverage_path", config); 85 | }; 86 | 87 | factory_->registerBuilder( 88 | "ComputeCoveragePath", builder); 89 | } 90 | 91 | static void TearDownTestCase() 92 | { 93 | factory_.reset(); 94 | action_server_.reset(); 95 | delete config_; 96 | config_ = nullptr; 97 | node_.reset(); 98 | } 99 | 100 | void TearDown() override 101 | { 102 | tree_.reset(); 103 | } 104 | 105 | static std::shared_ptr action_server_; 106 | 107 | protected: 108 | static rclcpp::Node::SharedPtr node_; 109 | static BT::NodeConfiguration * config_; 110 | static std::shared_ptr factory_; 111 | static std::shared_ptr tree_; 112 | }; 113 | 114 | rclcpp::Node::SharedPtr ComputeCoveragePathActionTestFixture::node_ = nullptr; 115 | std::shared_ptr 116 | ComputeCoveragePathActionTestFixture::action_server_ = nullptr; 117 | BT::NodeConfiguration * ComputeCoveragePathActionTestFixture::config_ = nullptr; 118 | std::shared_ptr ComputeCoveragePathActionTestFixture::factory_ = nullptr; 119 | std::shared_ptr ComputeCoveragePathActionTestFixture::tree_ = nullptr; 120 | 121 | TEST_F(ComputeCoveragePathActionTestFixture, test_tick) 122 | { 123 | // create tree 124 | std::string xml_txt = 125 | R"( 126 | 127 | 128 | 129 | 130 | )"; 131 | 132 | tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); 133 | 134 | // tick until node succeeds 135 | while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { 136 | tree_->rootNode()->executeTick(); 137 | } 138 | 139 | // the goal should have reached our server 140 | EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); 141 | 142 | // check if returned path is correct 143 | nav_msgs::msg::Path path; 144 | [[maybe_unused]] auto res = config_->blackboard->get("path", path); 145 | EXPECT_EQ(path.poses.size(), 2u); 146 | EXPECT_EQ(path.poses[0].pose.position.x, 0.0); 147 | EXPECT_EQ(path.poses[1].pose.position.x, 1.0); 148 | 149 | // halt node so another goal can be sent 150 | tree_->haltTree(); 151 | EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); 152 | } 153 | 154 | int main(int argc, char ** argv) 155 | { 156 | ::testing::InitGoogleTest(&argc, argv); 157 | 158 | // initialize ROS 159 | rclcpp::init(argc, argv); 160 | 161 | // initialize action server and spin on new thread 162 | ComputeCoveragePathActionTestFixture::action_server_ = 163 | std::make_shared(); 164 | 165 | std::thread server_thread([]() { 166 | rclcpp::spin(ComputeCoveragePathActionTestFixture::action_server_); 167 | }); 168 | 169 | int all_successful = RUN_ALL_TESTS(); 170 | 171 | // shutdown ROS 172 | rclcpp::shutdown(); 173 | server_thread.join(); 174 | 175 | return all_successful; 176 | } 177 | -------------------------------------------------------------------------------- /opennav_coverage_demo/README.md: -------------------------------------------------------------------------------- 1 | # Open Navigation's Nav2 Complete Coverage Demo 2 | 3 | This package contains the coverage navigator demo, using the Navigator, BT XML, BT Nodes, and Coverage Server to perform a simple coverage task. 4 | -------------------------------------------------------------------------------- /opennav_coverage_demo/launch/bringup_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Open Navigation LLC 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 17 | from launch.substitutions import LaunchConfiguration 18 | from launch_ros.actions import LoadComposableNodes 19 | from launch_ros.actions import Node 20 | from launch_ros.descriptions import ComposableNode, ParameterFile 21 | from nav2_common.launch import RewrittenYaml 22 | 23 | 24 | def generate_launch_description(): 25 | params_file = LaunchConfiguration('params_file') 26 | 27 | lifecycle_nodes = ['controller_server', 28 | 'bt_navigator', 29 | 'velocity_smoother', 30 | 'coverage_server'] 31 | 32 | remappings = [('/tf', 'tf'), 33 | ('/tf_static', 'tf_static')] 34 | 35 | # Create our own temporary YAML files that include substitutions 36 | autostart = True 37 | use_sim_time = True 38 | param_substitutions = { 39 | 'use_sim_time': str(use_sim_time), 40 | 'autostart': str(autostart)} 41 | 42 | configured_params = ParameterFile( 43 | RewrittenYaml( 44 | source_file=params_file, 45 | root_key='', 46 | param_rewrites=param_substitutions, 47 | convert_types=True), 48 | allow_substs=True) 49 | 50 | stdout_linebuf_envvar = SetEnvironmentVariable( 51 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 52 | 53 | declare_params_file_cmd = DeclareLaunchArgument('params_file') 54 | 55 | create_container = Node( 56 | name='nav2_container', 57 | package='rclcpp_components', 58 | executable='component_container_isolated', 59 | parameters=[configured_params, {'autostart': autostart}], 60 | remappings=remappings, 61 | output='screen') 62 | 63 | load_composable_nodes = LoadComposableNodes( 64 | target_container='nav2_container', 65 | composable_node_descriptions=[ 66 | ComposableNode( 67 | package='nav2_controller', 68 | plugin='nav2_controller::ControllerServer', 69 | name='controller_server', 70 | parameters=[configured_params], 71 | remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), 72 | ComposableNode( 73 | package='opennav_coverage', 74 | plugin='opennav_coverage::CoverageServer', 75 | name='coverage_server', 76 | parameters=[configured_params], 77 | remappings=remappings), 78 | ComposableNode( 79 | package='nav2_bt_navigator', 80 | plugin='nav2_bt_navigator::BtNavigator', 81 | name='bt_navigator', 82 | parameters=[configured_params], 83 | remappings=remappings), 84 | ComposableNode( 85 | package='nav2_velocity_smoother', 86 | plugin='nav2_velocity_smoother::VelocitySmoother', 87 | name='velocity_smoother', 88 | parameters=[configured_params], 89 | remappings=remappings + 90 | [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), 91 | ComposableNode( 92 | package='nav2_lifecycle_manager', 93 | plugin='nav2_lifecycle_manager::LifecycleManager', 94 | name='lifecycle_manager_navigation', 95 | parameters=[{'use_sim_time': use_sim_time, 96 | 'autostart': autostart, 97 | 'node_names': lifecycle_nodes}]), 98 | ], 99 | ) 100 | 101 | # # Create the launch description and populate 102 | ld = LaunchDescription() 103 | ld.add_action(stdout_linebuf_envvar) 104 | ld.add_action(declare_params_file_cmd) 105 | ld.add_action(create_container) 106 | ld.add_action(load_composable_nodes) 107 | return ld 108 | -------------------------------------------------------------------------------- /opennav_coverage_demo/launch/coverage_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Open Navigation LLC 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | import tempfile 17 | 18 | from ament_index_python.packages import get_package_share_directory 19 | from launch import LaunchDescription 20 | from launch.actions import ( 21 | ExecuteProcess, 22 | IncludeLaunchDescription, 23 | OpaqueFunction, 24 | RegisterEventHandler, 25 | ) 26 | from launch.event_handlers import OnShutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | 30 | 31 | def generate_launch_description(): 32 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 33 | coverage_demo_dir = get_package_share_directory('opennav_coverage_demo') 34 | rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz') 35 | sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') 36 | 37 | world = os.path.join(coverage_demo_dir, 'blank.world') 38 | param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml') 39 | robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') 40 | 41 | # start the simulation 42 | world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') 43 | world_sdf_xacro = ExecuteProcess(cmd=['xacro', '-o', world_sdf, 'headless:=false', world]) 44 | gazebo_server = ExecuteProcess( 45 | cmd=['gz', 'sim', '-r', '-s', world_sdf], 46 | output='screen', 47 | ) 48 | 49 | urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') 50 | with open(urdf, 'r') as infp: 51 | robot_description = infp.read() 52 | 53 | start_robot_state_publisher_cmd = Node( 54 | package='robot_state_publisher', 55 | executable='robot_state_publisher', 56 | name='robot_state_publisher', 57 | output='screen', 58 | parameters=[{'use_sim_time': True, 'robot_description': robot_description}], 59 | ) 60 | 61 | remove_temp_sdf_file = RegisterEventHandler( 62 | event_handler=OnShutdown( 63 | on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))] 64 | ) 65 | ) 66 | 67 | gazebo_client = IncludeLaunchDescription( 68 | PythonLaunchDescriptionSource( 69 | os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py') 70 | ), 71 | launch_arguments={'gz_args': ['-v4 -g ']}.items(), 72 | ) 73 | 74 | start_gazebo_spawner_cmd = IncludeLaunchDescription( 75 | PythonLaunchDescriptionSource(os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), 76 | launch_arguments={ 77 | 'namespace': '', 78 | 'robot_name': 'turtlebot3_waffle', 79 | 'robot_sdf': robot_sdf, 80 | 'x_pose': str(5.0), 81 | 'y_pose': str(5.0), 82 | 'z_pose': str(0.1), 83 | 'roll': str(0.0), 84 | 'pitch': str(0.0), 85 | 'yaw': str(0.0), 86 | }.items(), 87 | ) 88 | 89 | # start the visualization 90 | rviz_cmd = IncludeLaunchDescription( 91 | PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), 92 | launch_arguments={'namespace': '', 'rviz_config': rviz_config_file}.items(), 93 | ) 94 | 95 | # start navigation 96 | bringup_cmd = IncludeLaunchDescription( 97 | PythonLaunchDescriptionSource(os.path.join(coverage_demo_dir, 'bringup_launch.py')), 98 | launch_arguments={'params_file': param_file_path}.items(), 99 | ) 100 | 101 | # world->odom transform, no localization. For visualization & controller transform 102 | fake_localization_cmd = Node( 103 | package='tf2_ros', 104 | executable='static_transform_publisher', 105 | output='screen', 106 | arguments=['5.0', '5.0', '0', '0', '0', '0', 'map', 'odom'], 107 | ) 108 | 109 | # start the demo task 110 | demo_cmd = Node( 111 | package='opennav_coverage_demo', 112 | executable='demo_coverage', 113 | emulate_tty=True, 114 | output='screen', 115 | ) 116 | 117 | ld = LaunchDescription() 118 | ld.add_action(world_sdf_xacro) 119 | ld.add_action(remove_temp_sdf_file) 120 | ld.add_action(gazebo_server) 121 | ld.add_action(gazebo_client) 122 | ld.add_action(start_gazebo_spawner_cmd) 123 | 124 | ld.add_action(start_robot_state_publisher_cmd) 125 | ld.add_action(rviz_cmd) 126 | ld.add_action(bringup_cmd) 127 | ld.add_action(fake_localization_cmd) 128 | ld.add_action(demo_cmd) 129 | return ld 130 | -------------------------------------------------------------------------------- /opennav_coverage_demo/launch/row_bringup_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Open Navigation LLC 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 17 | from launch.substitutions import LaunchConfiguration 18 | from launch_ros.actions import LoadComposableNodes 19 | from launch_ros.actions import Node 20 | from launch_ros.descriptions import ComposableNode, ParameterFile 21 | from nav2_common.launch import RewrittenYaml 22 | 23 | 24 | def generate_launch_description(): 25 | params_file = LaunchConfiguration('params_file') 26 | 27 | lifecycle_nodes = ['controller_server', 28 | 'bt_navigator', 29 | 'velocity_smoother', 30 | 'row_coverage_server'] 31 | 32 | remappings = [('/tf', 'tf'), 33 | ('/tf_static', 'tf_static')] 34 | 35 | # Create our own temporary YAML files that include substitutions 36 | autostart = True 37 | use_sim_time = True 38 | param_substitutions = { 39 | 'use_sim_time': str(use_sim_time), 40 | 'autostart': str(autostart)} 41 | 42 | configured_params = ParameterFile( 43 | RewrittenYaml( 44 | source_file=params_file, 45 | root_key='', 46 | param_rewrites=param_substitutions, 47 | convert_types=True), 48 | allow_substs=True) 49 | 50 | stdout_linebuf_envvar = SetEnvironmentVariable( 51 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 52 | 53 | declare_params_file_cmd = DeclareLaunchArgument('params_file') 54 | 55 | create_container = Node( 56 | name='nav2_container', 57 | package='rclcpp_components', 58 | executable='component_container_isolated', 59 | parameters=[configured_params, {'autostart': autostart}], 60 | remappings=remappings, 61 | output='screen') 62 | 63 | load_composable_nodes = LoadComposableNodes( 64 | target_container='nav2_container', 65 | composable_node_descriptions=[ 66 | ComposableNode( 67 | package='nav2_controller', 68 | plugin='nav2_controller::ControllerServer', 69 | name='controller_server', 70 | parameters=[configured_params], 71 | remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), 72 | ComposableNode( 73 | package='opennav_row_coverage', 74 | plugin='opennav_row_coverage::RowCoverageServer', 75 | name='row_coverage_server', 76 | parameters=[configured_params], 77 | remappings=remappings), 78 | ComposableNode( 79 | package='nav2_bt_navigator', 80 | plugin='nav2_bt_navigator::BtNavigator', 81 | name='bt_navigator', 82 | parameters=[configured_params], 83 | remappings=remappings), 84 | ComposableNode( 85 | package='nav2_velocity_smoother', 86 | plugin='nav2_velocity_smoother::VelocitySmoother', 87 | name='velocity_smoother', 88 | parameters=[configured_params], 89 | remappings=remappings + 90 | [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), 91 | ComposableNode( 92 | package='nav2_lifecycle_manager', 93 | plugin='nav2_lifecycle_manager::LifecycleManager', 94 | name='lifecycle_manager_navigation', 95 | parameters=[{'use_sim_time': use_sim_time, 96 | 'autostart': autostart, 97 | 'node_names': lifecycle_nodes}]), 98 | ], 99 | ) 100 | 101 | # # Create the launch description and populate 102 | ld = LaunchDescription() 103 | ld.add_action(stdout_linebuf_envvar) 104 | ld.add_action(declare_params_file_cmd) 105 | ld.add_action(create_container) 106 | ld.add_action(load_composable_nodes) 107 | return ld 108 | -------------------------------------------------------------------------------- /opennav_coverage_demo/launch/row_coverage_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Open Navigation LLC 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | import tempfile 17 | 18 | from ament_index_python.packages import get_package_share_directory 19 | from launch import LaunchDescription 20 | from launch.actions import ( 21 | ExecuteProcess, 22 | IncludeLaunchDescription, 23 | OpaqueFunction, 24 | RegisterEventHandler, 25 | ) 26 | from launch.event_handlers import OnShutdown 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from launch_ros.actions import Node 29 | 30 | 31 | def generate_launch_description(): 32 | nav2_bringup_dir = get_package_share_directory('nav2_bringup') 33 | coverage_demo_dir = get_package_share_directory('opennav_coverage_demo') 34 | rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz') 35 | sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') 36 | 37 | world = os.path.join(coverage_demo_dir, 'blank.world') 38 | param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml') 39 | robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') 40 | 41 | # start the simulation 42 | world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') 43 | world_sdf_xacro = ExecuteProcess(cmd=['xacro', '-o', world_sdf, 'headless:=false', world]) 44 | gazebo_server = ExecuteProcess( 45 | cmd=['gz', 'sim', '-r', '-s', world_sdf], 46 | output='screen', 47 | ) 48 | 49 | urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') 50 | with open(urdf, 'r') as infp: 51 | robot_description = infp.read() 52 | 53 | start_robot_state_publisher_cmd = Node( 54 | package='robot_state_publisher', 55 | executable='robot_state_publisher', 56 | name='robot_state_publisher', 57 | output='screen', 58 | parameters=[{'use_sim_time': True, 'robot_description': robot_description}], 59 | ) 60 | 61 | remove_temp_sdf_file = RegisterEventHandler( 62 | event_handler=OnShutdown( 63 | on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))] 64 | ) 65 | ) 66 | 67 | gazebo_client = IncludeLaunchDescription( 68 | PythonLaunchDescriptionSource( 69 | os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py') 70 | ), 71 | launch_arguments={'gz_args': ['-v4 -g ']}.items(), 72 | ) 73 | 74 | start_gazebo_spawner_cmd = IncludeLaunchDescription( 75 | PythonLaunchDescriptionSource(os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), 76 | launch_arguments={ 77 | 'namespace': '', 78 | 'robot_name': 'turtlebot3_waffle', 79 | 'robot_sdf': robot_sdf, 80 | 'x_pose': str(6.23), 81 | 'y_pose': str(15.0), 82 | 'z_pose': str(0.1), 83 | 'roll': str(0.0), 84 | 'pitch': str(0.0), 85 | 'yaw': str(-1.5708), 86 | }.items(), 87 | ) 88 | 89 | # start the visualization 90 | rviz_cmd = IncludeLaunchDescription( 91 | PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), 92 | launch_arguments={'namespace': '', 'rviz_config': rviz_config_file}.items(), 93 | ) 94 | 95 | # start navigation 96 | bringup_cmd = IncludeLaunchDescription( 97 | PythonLaunchDescriptionSource(os.path.join(coverage_demo_dir, 'row_bringup_launch.py')), 98 | launch_arguments={'params_file': param_file_path}.items(), 99 | ) 100 | 101 | # Demo GPS->map->odom transform, no localization. For visualization & controller transform 102 | fake_localization_cmd = Node( 103 | package='tf2_ros', 104 | executable='static_transform_publisher', 105 | output='screen', 106 | arguments=['6.23', '15', '0', '0', '0', '0', 'map', 'odom'], 107 | ) 108 | fake_gps_cmd = Node( 109 | package='tf2_ros', 110 | executable='static_transform_publisher', 111 | output='screen', 112 | arguments=['0', '0', '0', '0', '0', '0', 'EPSG:4258', 'map'], 113 | ) 114 | 115 | # start the demo task 116 | demo_cmd = Node( 117 | package='opennav_coverage_demo', 118 | executable='demo_row_coverage', 119 | emulate_tty=True, 120 | output='screen', 121 | ) 122 | 123 | ld = LaunchDescription() 124 | ld.add_action(world_sdf_xacro) 125 | ld.add_action(remove_temp_sdf_file) 126 | ld.add_action(gazebo_server) 127 | ld.add_action(gazebo_client) 128 | ld.add_action(start_gazebo_spawner_cmd) 129 | 130 | ld.add_action(start_robot_state_publisher_cmd) 131 | ld.add_action(rviz_cmd) 132 | ld.add_action(bringup_cmd) 133 | ld.add_action(fake_localization_cmd) 134 | ld.add_action(fake_gps_cmd) 135 | ld.add_action(demo_cmd) 136 | return ld 137 | -------------------------------------------------------------------------------- /opennav_coverage_demo/opennav_coverage_demo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-navigation/opennav_coverage/77dcdca16f13f02d69a53af423c1ebc1b78e88ca/opennav_coverage_demo/opennav_coverage_demo/__init__.py -------------------------------------------------------------------------------- /opennav_coverage_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opennav_coverage_demo 5 | 0.0.1 6 | A demo using the Coverage Server, BT Nodes, and Coverage Navigator 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | rclcpp 11 | rclcpp_action 12 | nav2_util 13 | nav2_bringup 14 | opennav_coverage_msgs 15 | opennav_coverage_bt 16 | opennav_coverage 17 | opennav_row_coverage 18 | opennav_coverage_navigator 19 | 20 | nav2_minimal_tb3_sim 21 | 22 | ament_lint_common 23 | ament_lint_auto 24 | 25 | 26 | ament_python 27 | 28 | 29 | -------------------------------------------------------------------------------- /opennav_coverage_demo/params/demo_params.yaml: -------------------------------------------------------------------------------- 1 | coverage_server: 2 | ros__parameters: 3 | use_sim_time: True 4 | default_headland_width: 0.25 5 | robot_width: 0.7 6 | operation_width: 0.7 7 | min_turning_radius: 0.35 8 | linear_curv_change: 200.0 9 | coordinates_in_cartesian_frame: true 10 | default_allow_overlap: false 11 | default_swath_angle_type: "SET_ANGLE" 12 | default_swath_angle: 0.0 # We know its aligned for the demo 13 | 14 | row_coverage_server: 15 | ros__parameters: 16 | use_sim_time: True 17 | robot_width: 0.7 18 | operation_width: 0.7 19 | min_turning_radius: 0.35 20 | linear_curv_change: 200.0 21 | coordinates_in_cartesian_frame: true 22 | default_swath_type: "CENTER" 23 | default_offset: 0.0 24 | 25 | bt_navigator: 26 | ros__parameters: 27 | use_sim_time: True 28 | global_frame: map 29 | robot_base_frame: base_link 30 | odom_topic: /odom 31 | bt_loop_duration: 10 32 | default_server_timeout: 20 33 | # 'default_coverage_bt_xml' uses defaults: 34 | # opennav_coverage_bt/behavior_tree/navigate_w_basic_complete_coverage.xml 35 | navigators: ['navigate_complete_coverage'] 36 | navigate_complete_coverage: 37 | plugin: "opennav_coverage_navigator/CoverageNavigator" 38 | error_code_names: ["compute_path_error_code", "follow_path_error_code", "compute_coverage_error_code"] 39 | plugin_lib_names: 40 | - opennav_compute_complete_coverage_action_bt_node 41 | - opennav_cancel_complete_coverage_action_bt_node 42 | 43 | controller_server: 44 | ros__parameters: 45 | use_sim_time: True 46 | controller_frequency: 20.0 47 | min_x_velocity_threshold: 0.001 48 | min_y_velocity_threshold: 0.5 49 | min_theta_velocity_threshold: 0.001 50 | failure_tolerance: 0.3 51 | progress_checker_plugin: "progress_checker" 52 | goal_checker_plugins: ["general_goal_checker"] 53 | controller_plugins: ["FollowPath"] 54 | # Progress checker parameters 55 | progress_checker: 56 | plugin: "nav2_controller::SimpleProgressChecker" 57 | required_movement_radius: 0.5 58 | movement_time_allowance: 10.0 59 | # Goal checker parameters 60 | general_goal_checker: 61 | stateful: True 62 | plugin: "nav2_controller::SimpleGoalChecker" 63 | xy_goal_tolerance: 0.25 64 | yaw_goal_tolerance: 0.25 65 | # RPP parameters 66 | FollowPath: 67 | plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" 68 | desired_linear_vel: 0.5 69 | lookahead_dist: 0.6 70 | min_lookahead_dist: 0.3 71 | max_lookahead_dist: 0.9 72 | lookahead_time: 1.5 73 | rotate_to_heading_angular_vel: 1.8 74 | use_velocity_scaled_lookahead_dist: true 75 | min_approach_linear_velocity: 0.05 76 | approach_velocity_scaling_dist: 0.6 77 | use_collision_detection: true 78 | max_allowed_time_to_collision_up_to_carrot: 1.0 79 | use_regulated_linear_velocity_scaling: true 80 | use_fixed_curvature_lookahead: false 81 | curvature_lookahead_dist: 0.25 82 | use_cost_regulated_linear_velocity_scaling: false 83 | regulated_linear_scaling_min_radius: 0.9 84 | regulated_linear_scaling_min_speed: 0.25 85 | use_rotate_to_heading: false # allows to follow coverage path better in sharper turns 86 | allow_reversing: false 87 | rotate_to_heading_min_angle: 0.785 88 | max_angular_accel: 3.2 89 | max_robot_pose_search_dist: 10.0 90 | use_interpolation: true 91 | 92 | local_costmap: 93 | local_costmap: 94 | ros__parameters: 95 | update_frequency: 5.0 96 | publish_frequency: 2.0 97 | global_frame: odom 98 | robot_base_frame: base_link 99 | use_sim_time: True 100 | rolling_window: true 101 | width: 3 102 | height: 3 103 | resolution: 0.05 104 | footprint: "[ [0.5, 0.25], [0.5, -0.25], [-0.5, -0.25], [-0.5, 0.25] ]" 105 | plugins: ["voxel_layer", "inflation_layer"] 106 | inflation_layer: 107 | plugin: "nav2_costmap_2d::InflationLayer" 108 | cost_scaling_factor: 3.0 109 | inflation_radius: 0.55 110 | voxel_layer: 111 | plugin: "nav2_costmap_2d::ObstacleLayer" 112 | enabled: True 113 | max_obstacle_height: 2.0 114 | mark_threshold: 0 115 | footprint_clearing_enabled: true 116 | observation_sources: scan 117 | scan: 118 | topic: /scan 119 | max_obstacle_height: 2.0 120 | clearing: True 121 | marking: True 122 | data_type: "LaserScan" 123 | raytrace_max_range: 3.0 124 | raytrace_min_range: 0.0 125 | obstacle_max_range: 2.5 126 | obstacle_min_range: 0.2 127 | static_layer: 128 | plugin: "nav2_costmap_2d::StaticLayer" 129 | map_subscribe_transient_local: True 130 | always_send_full_costmap: True 131 | 132 | robot_state_publisher: 133 | ros__parameters: 134 | use_sim_time: True 135 | 136 | bt_navigator_navigate_complete_coverage_rclcpp_node: 137 | ros__parameters: 138 | use_sim_time: True 139 | 140 | velocity_smoother: 141 | ros__parameters: 142 | use_sim_time: True 143 | smoothing_frequency: 20.0 144 | scale_velocities: False 145 | feedback: "OPEN_LOOP" 146 | max_velocity: [0.5, 0.0, 2.0] 147 | min_velocity: [-0.5, 0.0, -2.0] 148 | max_accel: [2.5, 0.0, 5.2] 149 | max_decel: [-2.5, 0.0, -5.2] 150 | odom_topic: "odom" 151 | odom_duration: 0.1 152 | deadband_velocity: [0.0, 0.0, 0.0] 153 | velocity_timeout: 1.0 154 | -------------------------------------------------------------------------------- /opennav_coverage_demo/pytest.ini: -------------------------------------------------------------------------------- 1 | [pytest] 2 | junit_family=xunit2 3 | -------------------------------------------------------------------------------- /opennav_coverage_demo/resource/opennav_coverage_demo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-navigation/opennav_coverage/77dcdca16f13f02d69a53af423c1ebc1b78e88ca/opennav_coverage_demo/resource/opennav_coverage_demo -------------------------------------------------------------------------------- /opennav_coverage_demo/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/opennav_coverage_demo 3 | [install] 4 | install_scripts=$base/lib/opennav_coverage_demo 5 | -------------------------------------------------------------------------------- /opennav_coverage_demo/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | 4 | from setuptools import setup 5 | 6 | 7 | package_name = 'opennav_coverage_demo' 8 | 9 | setup( 10 | name=package_name, 11 | version='1.0.0', 12 | packages=[package_name], 13 | data_files=[ 14 | ('share/ament_index/resource_index/packages', 15 | ['resource/' + package_name]), 16 | ('share/' + package_name, ['package.xml']), 17 | (os.path.join('share', package_name), glob('launch/*')), 18 | (os.path.join('share', package_name), glob('world/*')), 19 | (os.path.join('share', package_name), glob('params/*')), 20 | ], 21 | install_requires=['setuptools'], 22 | zip_safe=True, 23 | maintainer='steve', 24 | maintainer_email='stevenmacenski@gmail.com', 25 | description='An importable library for writing mobile robot applications in python3', 26 | license='Apache-2.0', 27 | tests_require=['pytest'], 28 | entry_points={ 29 | 'console_scripts': [ 30 | 'demo_coverage = opennav_coverage_demo.demo_coverage:main', 31 | 'demo_row_coverage = opennav_coverage_demo.demo_row_coverage:main', 32 | ], 33 | }, 34 | ) 35 | -------------------------------------------------------------------------------- /opennav_coverage_demo/test/demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-navigation/opennav_coverage/77dcdca16f13f02d69a53af423c1ebc1b78e88ca/opennav_coverage_demo/test/demo.png -------------------------------------------------------------------------------- /opennav_coverage_demo/test/demo_rows.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-navigation/opennav_coverage/77dcdca16f13f02d69a53af423c1ebc1b78e88ca/opennav_coverage_demo/test/demo_rows.png -------------------------------------------------------------------------------- /opennav_coverage_demo/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /opennav_coverage_demo/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /opennav_coverage_demo/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /opennav_coverage_demo/world/blank.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0 8 | 0 0 10 0 0 0 9 | 0.8 0.8 0.8 1 10 | 0.8 0.8 0.8 1 11 | 12 | 1000 13 | 0.9 14 | 0.01 15 | 0.001 16 | 17 | -0.5 0.1 -0.9 18 | 19 | 20 | 1 21 | 22 | 23 | 24 | 25 | 0 0 1 26 | 100 100 27 | 28 | 29 | 10 30 | 31 | 32 | 33 | 34 | 0 0 1 35 | 100 100 36 | 37 | 38 | 39 | 0.8 0.8 0.8 1 40 | 0.8 0.8 0.8 1 41 | 0.8 0.8 0.8 1 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(opennav_coverage_msgs) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(builtin_interfaces REQUIRED) 6 | find_package(nav_msgs REQUIRED) 7 | find_package(geometry_msgs REQUIRED) 8 | find_package(rosidl_default_generators REQUIRED) 9 | find_package(std_msgs REQUIRED) 10 | find_package(action_msgs REQUIRED) 11 | 12 | rosidl_generate_interfaces(${PROJECT_NAME} 13 | "msg/Coordinate.msg" 14 | "msg/Coordinates.msg" 15 | "msg/HeadlandMode.msg" 16 | "msg/SwathMode.msg" 17 | "msg/RowSwathMode.msg" 18 | "msg/RouteMode.msg" 19 | "msg/PathMode.msg" 20 | "msg/Swath.msg" 21 | "msg/PathComponents.msg" 22 | "action/ComputeCoveragePath.action" 23 | "action/NavigateCompleteCoverage.action" 24 | DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs 25 | ) 26 | 27 | ament_export_dependencies(rosidl_default_runtime) 28 | 29 | ament_package() 30 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Open Navigation's Nav2 Coverage Msgs 2 | 3 | This package contains a set of messages for coverage planning and navigation 4 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/action/ComputeCoveragePath.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | 3 | # Whether to perform all 4 stages: Headlands, Swath (Required), Route, Path 4 | bool generate_headland True 5 | bool generate_route True 6 | bool generate_path True 7 | 8 | # The field specification to use. 9 | # If using polygons, bounding polygon must be first, followed by inner cutouts 10 | # Both must specify if the data is cartesian or GPS coordinates 11 | # If using Row Coverage Server, must use gml field. 12 | bool use_gml_file False 13 | string gml_field 14 | opennav_coverage_msgs/Coordinates[] polygons 15 | string frame_id map 16 | 17 | # Modes of operation of each stage, if used 18 | opennav_coverage_msgs/HeadlandMode headland_mode 19 | opennav_coverage_msgs/SwathMode swath_mode 20 | opennav_coverage_msgs/RowSwathMode row_swath_mode 21 | opennav_coverage_msgs/RouteMode route_mode 22 | opennav_coverage_msgs/PathMode path_mode 23 | 24 | --- 25 | #result definition 26 | 27 | # Error codes 28 | # Note: The expected priority order of the errors should match the message order 29 | uint16 NONE=0 30 | uint16 INTERNAL_F2C_ERROR=801 31 | uint16 INVALID_MODE_SET=802 32 | uint16 INVALID_REQUEST=803 33 | uint16 INVALID_COORDS=803 34 | 35 | nav_msgs/Path nav_path 36 | opennav_coverage_msgs/PathComponents coverage_path 37 | builtin_interfaces/Duration planning_time 38 | uint16 error_code 39 | 40 | --- 41 | #feedback definition 42 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/action/NavigateCompleteCoverage.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | 3 | # Define the field as either a filepath to a GML file or as a vector of polygons 4 | # Whereas the first polygon is the outer field and subsequent polygons are internal voids. 5 | # When both are specified, the file is used in ComputeCoveragePath BT Node. 6 | string field_filepath 7 | geometry_msgs/Polygon[] polygons 8 | string frame_id map # Specify the frame of reference of the polygon field 9 | 10 | string behavior_tree 11 | --- 12 | #result definition 13 | 14 | # Error codes 15 | # Note: The expected priority order of the errors should match the message order 16 | uint16 NONE=0 17 | 18 | uint16 error_code 19 | --- 20 | #feedback definition 21 | 22 | geometry_msgs/PoseStamped current_pose 23 | builtin_interfaces/Duration navigation_time 24 | builtin_interfaces/Duration estimated_time_remaining 25 | int16 number_of_recoveries 26 | float32 distance_remaining 27 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/Coordinate.msg: -------------------------------------------------------------------------------- 1 | # Can be GPS, Cartesian, or any other kind of coordinate system 2 | float32 axis1 3 | float32 axis2 -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/Coordinates.msg: -------------------------------------------------------------------------------- 1 | opennav_coverage_msgs/Coordinate[] coordinates 2 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/HeadlandMode.msg: -------------------------------------------------------------------------------- 1 | string mode "UNKNOWN" 2 | 3 | # Specific mode setting 4 | float32 width 2.0 # width of headland around field 5 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/PathComponents.msg: -------------------------------------------------------------------------------- 1 | # An ordered set of swaths (if route generated) and turns (if path generated) 2 | std_msgs/Header header 3 | opennav_coverage_msgs/Swath[] swaths 4 | nav_msgs/Path[] turns 5 | bool contains_turns 6 | bool swaths_ordered 7 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/PathMode.msg: -------------------------------------------------------------------------------- 1 | string mode "UNKNOWN" # DUBIN, REEDS_SHEPP 2 | string continuity_mode "UNKNOWN" # CONTINUOUS, DISCONTINUOUS 3 | float32 turn_point_distance 0.1 # Distance between path points in non-swath turns 4 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/RouteMode.msg: -------------------------------------------------------------------------------- 1 | string mode "UNKNOWN" # BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM 2 | 3 | # Specific mode setting 4 | uint16 spiral_n 4 # If mode=SPIRAL, this is the number of swaths to spiral 5 | uint16[] custom_order # If mode=CUSTOM, this is the order of swaths to set. Must be specified. 6 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/RowSwathMode.msg: -------------------------------------------------------------------------------- 1 | string mode "UNKNOWN" # CENTER, OFFSET, ROWSARESWATHS 2 | int32[] skip_ids # The ids to skip 3 | 4 | # Specific mode settings 5 | float32 offset 0.0 # If mode=OFFSET for offset for swath relative to rows 6 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/Swath.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point32 start 2 | geometry_msgs/Point32 end 3 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/msg/SwathMode.msg: -------------------------------------------------------------------------------- 1 | string objective "UNKNOWN" # LENGTH, NUMBER, or COVERAGE 2 | string mode "UNKNOWN" # BRUTE_FORCE, SET_ANGLE 3 | 4 | # Specific mode settings 5 | float32 best_angle 0.0 # If mode=SET_ANGLE for angle to generate swaths based on 6 | float32 step_angle 1.7453e-2 # If mode=BRUTE_FORCE, angular step to search 7 | -------------------------------------------------------------------------------- /opennav_coverage_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opennav_coverage_msgs 5 | 0.0.1 6 | A set of ROS interfaces for complete coverage planning 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | rclcpp_lifecycle 15 | nav2_util 16 | nav2_msgs 17 | nav_msgs 18 | geometry_msgs 19 | builtin_interfaces 20 | tf2_ros 21 | rosidl_default_generators 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | 26 | rosidl_interface_packages 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /opennav_coverage_navigator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(opennav_coverage_navigator) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(rclcpp_action REQUIRED) 7 | find_package(std_msgs REQUIRED) 8 | find_package(nav2_util REQUIRED) 9 | find_package(nav2_msgs REQUIRED) 10 | find_package(nav2_core REQUIRED) 11 | find_package(nav2_behavior_tree REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(opennav_coverage_msgs REQUIRED) 15 | 16 | # potentially replace with nav2_common, nav2_package() 17 | set(CMAKE_CXX_STANDARD 17) 18 | add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wnull-dereference) 19 | 20 | include_directories( 21 | include 22 | ) 23 | 24 | set(dependencies 25 | rclcpp 26 | rclcpp_action 27 | std_msgs 28 | nav2_util 29 | nav2_msgs 30 | nav2_core 31 | nav2_behavior_tree 32 | nav_msgs 33 | geometry_msgs 34 | opennav_coverage_msgs 35 | ) 36 | 37 | include_directories( 38 | include 39 | ) 40 | 41 | add_library(opennav_coverage_navigator SHARED src/coverage_navigator.cpp) 42 | ament_target_dependencies(opennav_coverage_navigator ${dependencies}) 43 | 44 | pluginlib_export_plugin_description_file(nav2_core navigator_plugins.xml) 45 | 46 | install(TARGETS opennav_coverage_navigator 47 | ARCHIVE DESTINATION lib 48 | LIBRARY DESTINATION lib 49 | RUNTIME DESTINATION bin 50 | ) 51 | 52 | install(DIRECTORY include/ 53 | DESTINATION include/ 54 | ) 55 | 56 | if(BUILD_TESTING) 57 | find_package(ament_lint_auto REQUIRED) 58 | find_package(ament_cmake_gtest REQUIRED) 59 | ament_lint_auto_find_test_dependencies() 60 | add_subdirectory(test) 61 | endif() 62 | 63 | ament_export_include_directories(include) 64 | ament_export_libraries(opennav_coverage_navigator) 65 | ament_export_dependencies(${dependencies}) 66 | ament_package() 67 | -------------------------------------------------------------------------------- /opennav_coverage_navigator/README.md: -------------------------------------------------------------------------------- 1 | # Open Navigation's Nav2 Complete Coverage Server 2 | 3 | This package contains the coverage navigator usign the coverage server for high level navigation requests 4 | -------------------------------------------------------------------------------- /opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_COVERAGE_NAVIGATOR__COVERAGE_NAVIGATOR_HPP_ 16 | #define OPENNAV_COVERAGE_NAVIGATOR__COVERAGE_NAVIGATOR_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "rclcpp/rclcpp.hpp" 23 | #include "rclcpp_action/rclcpp_action.hpp" 24 | #include "geometry_msgs/msg/pose_stamped.hpp" 25 | #include "nav2_core/behavior_tree_navigator.hpp" 26 | #include "opennav_coverage_msgs/action/navigate_complete_coverage.hpp" 27 | #include "nav2_util/geometry_utils.hpp" 28 | #include "nav2_util/robot_utils.hpp" 29 | #include "nav_msgs/msg/path.hpp" 30 | #include "nav2_util/odometry_utils.hpp" 31 | 32 | namespace opennav_coverage_navigator 33 | { 34 | 35 | /** 36 | * @class CoverageNavigator 37 | * @brief A navigator for navigating to complete a coverage pattern 38 | */ 39 | class CoverageNavigator 40 | : public nav2_core::BehaviorTreeNavigator< 41 | opennav_coverage_msgs::action::NavigateCompleteCoverage> 42 | { 43 | public: 44 | using ActionT = opennav_coverage_msgs::action::NavigateCompleteCoverage; 45 | 46 | /** 47 | * @brief A constructor for CoverageNavigator 48 | */ 49 | CoverageNavigator() 50 | : BehaviorTreeNavigator() {} 51 | 52 | /** 53 | * @brief A configure state transition to configure navigator's state 54 | * @param node Weakptr to the lifecycle node 55 | * @param odom_smoother Object to get current smoothed robot's speed 56 | */ 57 | bool configure( 58 | rclcpp_lifecycle::LifecycleNode::WeakPtr node, 59 | std::shared_ptr odom_smoother) override; 60 | 61 | /** 62 | * @brief A cleanup state transition to remove memory allocated 63 | */ 64 | bool cleanup() override; 65 | 66 | /** 67 | * @brief Get action name for this navigator 68 | * @return string Name of action server 69 | */ 70 | std::string getName() override {return std::string("navigate_complete_coverage");} 71 | 72 | /** 73 | * @brief Get navigator's default BT 74 | * @param node WeakPtr to the lifecycle node 75 | * @return string Filepath to default XML 76 | */ 77 | std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 78 | 79 | protected: 80 | /** 81 | * @brief A callback to be called when a new goal is received by the BT action server 82 | * Can be used to check if goal is valid and put values on 83 | * the blackboard which depend on the received goal 84 | * @param goal Action template's goal message 85 | * @return bool if goal was received successfully to be processed 86 | */ 87 | bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override; 88 | 89 | /** 90 | * @brief A callback that defines execution that happens on one iteration through the BT 91 | * Can be used to publish action feedback 92 | */ 93 | void onLoop() override; 94 | 95 | /** 96 | * @brief A callback that is called when a preempt is requested 97 | */ 98 | void onPreempt(ActionT::Goal::ConstSharedPtr goal) override; 99 | 100 | /** 101 | * @brief A callback that is called when a the action is completed, can fill in 102 | * action result message or indicate that this action is done. 103 | * @param result Action template result message to populate 104 | * @param final_bt_status Resulting status of the behavior tree execution that may be 105 | * referenced while populating the result. 106 | */ 107 | void goalCompleted( 108 | typename ActionT::Result::SharedPtr result, 109 | const nav2_behavior_tree::BtStatus final_bt_status) override; 110 | 111 | /** 112 | * @brief Goal pose initialization on the blackboard 113 | * @param goal Action template's goal message to process 114 | */ 115 | void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); 116 | 117 | rclcpp::Time start_time_; 118 | std::string path_blackboard_id_, field_blackboard_id_, polygon_blackboard_id_; 119 | std::string polygon_frame_blackboard_id_; 120 | 121 | // Odometry smoother object 122 | std::shared_ptr odom_smoother_; 123 | }; 124 | 125 | } // namespace opennav_coverage_navigator 126 | 127 | #endif // OPENNAV_COVERAGE_NAVIGATOR__COVERAGE_NAVIGATOR_HPP_ 128 | -------------------------------------------------------------------------------- /opennav_coverage_navigator/navigator_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | Coverage Navigator 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /opennav_coverage_navigator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opennav_coverage_navigator 5 | 0.0.1 6 | A navigator plugin for BT Navigator for coverage planning 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | nav2_behavior_tree 15 | nav2_util 16 | nav2_core 17 | nav2_msgs 18 | nav_msgs 19 | geometry_msgs 20 | opennav_coverage_msgs 21 | opennav_coverage_bt 22 | opennav_coverage 23 | 24 | ament_lint_common 25 | ament_lint_auto 26 | opennav_coverage_bt 27 | opennav_coverage 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /opennav_coverage_navigator/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_coverage_navigator test_coverage_navigator.cpp) 2 | target_link_libraries(test_coverage_navigator opennav_coverage_navigator) 3 | ament_target_dependencies(test_coverage_navigator ${dependencies}) 4 | -------------------------------------------------------------------------------- /opennav_row_coverage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(opennav_row_coverage) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(rclcpp_components REQUIRED) 7 | find_package(rclcpp_lifecycle REQUIRED) 8 | find_package(nav2_util REQUIRED) 9 | find_package(Fields2Cover REQUIRED) 10 | find_package(opennav_coverage REQUIRED) 11 | find_package(opennav_coverage_msgs REQUIRED) 12 | find_package(tinyxml2 REQUIRED) 13 | 14 | # potentially replace with nav2_common, nav2_package() 15 | set(CMAKE_CXX_STANDARD 17) 16 | add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) 17 | 18 | include_directories( 19 | include 20 | ) 21 | set(executable_name opennav_row_coverage) 22 | set(library_name ${executable_name}_core) 23 | 24 | set(dependencies 25 | nav2_util 26 | rclcpp 27 | rclcpp_components 28 | rclcpp_lifecycle 29 | tf2_ros 30 | builtin_interfaces 31 | geometry_msgs 32 | nav2_msgs 33 | nav_msgs 34 | visualization_msgs 35 | std_msgs 36 | opennav_coverage 37 | opennav_coverage_msgs 38 | ) 39 | 40 | add_library(${library_name} SHARED 41 | src/row_swath_generator.cpp 42 | src/row_coverage_server.cpp 43 | ) 44 | 45 | ament_target_dependencies(${library_name} 46 | ${dependencies} 47 | ) 48 | 49 | target_link_libraries(${library_name} Fields2Cover tinyxml2::tinyxml2) 50 | 51 | add_executable(${executable_name} 52 | src/main.cpp 53 | ) 54 | 55 | target_link_libraries(${executable_name} ${library_name}) 56 | 57 | ament_target_dependencies(${executable_name} 58 | ${dependencies} 59 | ) 60 | 61 | target_link_libraries(${executable_name} Fields2Cover tinyxml2::tinyxml2) 62 | 63 | rclcpp_components_register_nodes(${library_name} "opennav_row_coverage::RowCoverageServer") 64 | 65 | install(TARGETS ${library_name} 66 | ARCHIVE DESTINATION lib 67 | LIBRARY DESTINATION lib 68 | RUNTIME DESTINATION bin 69 | ) 70 | 71 | install(TARGETS ${executable_name} 72 | RUNTIME DESTINATION lib/${PROJECT_NAME} 73 | ) 74 | 75 | install(DIRECTORY include/ 76 | DESTINATION include/ 77 | ) 78 | 79 | if(BUILD_TESTING) 80 | find_package(ament_lint_auto REQUIRED) 81 | find_package(ament_cmake_gtest REQUIRED) 82 | ament_lint_auto_find_test_dependencies() 83 | add_subdirectory(test) 84 | endif() 85 | 86 | ament_export_include_directories(include) 87 | ament_export_libraries(${library_name}) 88 | ament_export_dependencies(${dependencies}) 89 | ament_package() 90 | -------------------------------------------------------------------------------- /opennav_row_coverage/include/opennav_row_coverage/row_coverage_server.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_ROW_COVERAGE__ROW_COVERAGE_SERVER_HPP_ 16 | #define OPENNAV_ROW_COVERAGE__ROW_COVERAGE_SERVER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "fields2cover.h" // NOLINT 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_util/node_utils.hpp" 26 | #include "nav2_util/simple_action_server.hpp" 27 | #include "opennav_row_coverage/row_swath_generator.hpp" 28 | #include "opennav_coverage/route_generator.hpp" 29 | #include "opennav_coverage/path_generator.hpp" 30 | #include "opennav_coverage/visualizer.hpp" 31 | 32 | namespace opennav_row_coverage 33 | { 34 | /** 35 | * @class opennav_row_coverage::RowCoverageServer 36 | * @brief An action server which implements highly reconfigurable complete 37 | * coverage planning using the Fields2Cover library with pre-established rows 38 | */ 39 | class RowCoverageServer : public nav2_util::LifecycleNode 40 | { 41 | public: 42 | using ActionServer = nav2_util::SimpleActionServer; 43 | 44 | /** 45 | * @brief A constructor for opennav_row_coverage::RowCoverageServer 46 | * @param options Additional options to control creation of the node. 47 | */ 48 | explicit RowCoverageServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 49 | 50 | /** 51 | * @brief A destructor for opennav_row_coverage::RowCoverageServer 52 | */ 53 | ~RowCoverageServer() = default; 54 | 55 | protected: 56 | /** 57 | * @brief Main action callback method to complete action request 58 | */ 59 | void computeCoveragePath(); 60 | 61 | /** 62 | * @brief Validate the goal settings to know if valid to execute 63 | * @param Goal request to validate 64 | * @return SUCCESS or FAILURE 65 | */ 66 | bool validateGoal(typename std::shared_ptr req); 67 | 68 | /** 69 | * @brief Gets a preempted goal if immediately requested 70 | * @param Goal goal to check or replace if required with preemption 71 | * @return SUCCESS or FAILURE 72 | */ 73 | void getPreemptedGoalIfRequested( 74 | typename std::shared_ptr goal); 75 | 76 | /** 77 | * @brief Configure member variables 78 | * @param state Reference to LifeCycle node state 79 | * @return SUCCESS or FAILURE 80 | */ 81 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 82 | 83 | /** 84 | * @brief Activate member variables 85 | * @param state Reference to LifeCycle node state 86 | * @return SUCCESS or FAILURE 87 | */ 88 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 89 | 90 | /** 91 | * @brief Deactivate member variables 92 | * @param state Reference to LifeCycle node state 93 | * @return SUCCESS or FAILURE 94 | */ 95 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 96 | 97 | /** 98 | * @brief Reset member variables 99 | * @param state Reference to LifeCycle node state 100 | * @return SUCCESS or FAILURE 101 | */ 102 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 103 | 104 | /** 105 | * @brief Called when in shutdown state 106 | * @param state Reference to LifeCycle node state 107 | * @return SUCCESS or FAILURE 108 | */ 109 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 110 | 111 | /** 112 | * @brief Callback executed when a parameter change is detected 113 | * @param event ParameterEvent message 114 | */ 115 | rcl_interfaces::msg::SetParametersResult 116 | dynamicParametersCallback(std::vector parameters); 117 | 118 | // Dynamic parameters handler 119 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 120 | std::mutex dynamic_params_lock_; 121 | 122 | std::unique_ptr action_server_; 123 | 124 | std::unique_ptr robot_params_; 125 | std::unique_ptr swath_gen_; 126 | std::unique_ptr route_gen_; 127 | std::unique_ptr path_gen_; 128 | std::unique_ptr visualizer_; 129 | bool cartesian_frame_, order_ids_; 130 | }; 131 | 132 | } // namespace opennav_row_coverage 133 | 134 | #endif // OPENNAV_ROW_COVERAGE__ROW_COVERAGE_SERVER_HPP_ 135 | -------------------------------------------------------------------------------- /opennav_row_coverage/include/opennav_row_coverage/row_swath_generator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_ROW_COVERAGE__ROW_SWATH_GENERATOR_HPP_ 16 | #define OPENNAV_ROW_COVERAGE__ROW_SWATH_GENERATOR_HPP_ 17 | 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "opennav_row_coverage/types.hpp" 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "nav2_util/node_utils.hpp" 26 | #include "opennav_coverage/utils.hpp" 27 | #include "opennav_row_coverage/utils.hpp" 28 | 29 | namespace opennav_row_coverage 30 | { 31 | 32 | using namespace opennav_coverage; // NOLINT 33 | 34 | /** 35 | * @class An object to contain the relevent implementations of the swath generation policies 36 | * to be called within the swath row generator object with the appropriate parameters 37 | */ 38 | class SwathFactory 39 | { 40 | public: 41 | /** 42 | * @brief Computes swaths in the center of the marked rows 43 | * @param rows Rows to generate swaths from 44 | * @param ids_to_skip The ids to skip 45 | */ 46 | Swaths generateCenterSwaths(const Rows & rows, const SkipIDs & ids_to_skip); 47 | 48 | /** 49 | * @brief Computes swaths offset of the marked rows by a set amount 50 | * @param rows Rows to generate swaths from 51 | * @param offset Offset to use w.r.t. row 52 | * @return Swaths The generated swaths 53 | */ 54 | Swaths generateOffsetSwaths(const Rows & rows, const SkipIDs & ids_to_skip, float offset); 55 | 56 | /** 57 | * @brief Create Swaths directly from the rows 58 | * @param rows Rows to generate swaths from 59 | * @param ids The ids to skip 60 | * @return Swaths The generated swaths 61 | */ 62 | Swaths generateRowsAreSwaths(const Rows & rows, const SkipIDs & ids_to_skip); 63 | 64 | private: 65 | /** 66 | * @brief Determine if a id should be skipped 67 | * @param ids_to_skip The ids to skip 68 | * @param id The id to check 69 | * @return true If the id is in the ids_to_skip list 70 | */ 71 | bool skipId(const SkipIDs & ids_to_skip, int id); 72 | 73 | /** 74 | * @brief Calculate the width between two rows 75 | * @param row1 The first row 76 | * @param row2 The second row 77 | * @return double The width 78 | */ 79 | double calculateWidth(const LineString & row1, const LineString & row2); 80 | }; 81 | 82 | /** 83 | * @class An object to compute swaths given a set of pre-computed or defined rows 84 | * based on a policy about what the rows represent (e.g. if rows are crop, it should select 85 | * if we should use the center of the rows, use an offset, if the rows themselves 86 | * are the objective, etc) 87 | */ 88 | class RowSwathGenerator 89 | { 90 | public: 91 | /** 92 | * @brief Constructor for swath generator 93 | * @param node A node to get the swath type from 94 | */ 95 | template 96 | explicit RowSwathGenerator(const NodeT & node) 97 | { 98 | logger_ = node->get_logger(); 99 | nav2_util::declare_parameter_if_not_declared( 100 | node, "default_swath_type", rclcpp::ParameterValue("CENTER")); 101 | std::string type_str = node->get_parameter("default_swath_type").as_string(); 102 | default_type_ = toType(type_str); 103 | 104 | nav2_util::declare_parameter_if_not_declared( 105 | node, "default_offset", rclcpp::ParameterValue(0.5)); 106 | default_offset_ = static_cast(node->get_parameter("default_offset").as_double()); 107 | } 108 | 109 | /** 110 | * @brief Main method to generate the swaths 111 | * @param rows The rows of obstacles in the field 112 | * @param settings Action requested settings to use 113 | * @return Swaths The generated swaths 114 | */ 115 | Swaths generateSwaths( 116 | const Rows & rows, 117 | const opennav_coverage_msgs::msg::RowSwathMode & settings); 118 | 119 | /** 120 | * @brief Sets the mode manually of the swath mode for dynamic parameters 121 | * @param mode String for mode to use 122 | */ 123 | void setMode(const std::string & new_mode); 124 | 125 | /** 126 | * @brief Sets the offset manually for dynamic parameters 127 | * @param offset Offset to use 128 | */ 129 | void setOffset(const float offset) {default_offset_ = offset;} 130 | 131 | private: 132 | /** 133 | * @brief Convert the swath type to a string 134 | * @param swath_type The type of swath to generate 135 | * @return std::string The swath type as a string 136 | */ 137 | std::string toString(const RowSwathType & swath_type); 138 | 139 | /** 140 | * @brief Convert a string to a swath type 141 | * @param str The string to convert 142 | * @return RowSwathType The swath type 143 | */ 144 | RowSwathType toType(const std::string & str); 145 | 146 | /** 147 | * @brief Manually correct annotated row orientations for consistent operations 148 | * @param rows The rows of obstacles in the field 149 | * @return Rows with orientations made consistent 150 | */ 151 | Rows adjustRowOrientations(const Rows & rows); 152 | 153 | SwathFactory swath_factory_; 154 | RowSwathType default_type_; 155 | float default_offset_; 156 | 157 | rclcpp::Logger logger_{rclcpp::get_logger("SwathGenerator")}; 158 | }; 159 | 160 | } // namespace opennav_row_coverage 161 | 162 | #endif // OPENNAV_ROW_COVERAGE__ROW_SWATH_GENERATOR_HPP_ 163 | -------------------------------------------------------------------------------- /opennav_row_coverage/include/opennav_row_coverage/types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_ROW_COVERAGE__TYPES_HPP_ 16 | #define OPENNAV_ROW_COVERAGE__TYPES_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "opennav_coverage/types.hpp" 22 | 23 | namespace opennav_row_coverage 24 | { 25 | 26 | typedef std::pair Row; 27 | typedef std::vector Rows; 28 | typedef std::vector SkipIDs; 29 | 30 | enum class RowSwathType 31 | { 32 | UNKNOWN = 0, 33 | CENTER = 1, 34 | OFFSET = 2, 35 | ROWSARESWATHS = 3 36 | }; 37 | 38 | } // namespace opennav_row_coverage 39 | 40 | #endif // OPENNAV_ROW_COVERAGE__TYPES_HPP_ 41 | -------------------------------------------------------------------------------- /opennav_row_coverage/include/opennav_row_coverage/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef OPENNAV_ROW_COVERAGE__UTILS_HPP_ 16 | #define OPENNAV_ROW_COVERAGE__UTILS_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "tinyxml2.h" // NOLINT 24 | #include "opennav_coverage/types.hpp" 25 | #include "opennav_row_coverage/types.hpp" 26 | 27 | namespace opennav_row_coverage 28 | { 29 | 30 | namespace util 31 | { 32 | 33 | using namespace opennav_coverage; // NOLINT 34 | 35 | /** 36 | * @brief Transform rows to a new coordinate system and a new reference. 37 | * Note: The field must be transformed to UTM before this operation. 38 | * @param rows The rows to transform 39 | * @param field The field holds the reference point and coordinate systems 40 | * @return rows transformed by the transformation of the field's coordinates 41 | */ 42 | inline Rows transformRowsWithRef( 43 | const Rows & rows, 44 | const F2CField & field) 45 | { 46 | auto coords = f2c::Transform::generateCoordTransf(field.getPrevCRS(), field.getCRS()); 47 | Rows trans_rows; 48 | for (const auto & row : rows) { 49 | LineString new_row = row.first.clone(); 50 | for (auto & point : new_row) { 51 | point->transform(coords.get()); 52 | point = point - field.getRefPoint(); 53 | } 54 | trans_rows.emplace_back(new_row, row.second); 55 | } 56 | return trans_rows; 57 | } 58 | 59 | /** 60 | * @brief Removes the field's reference point from the rows so that 61 | * All F2C operations continue to reference the axial point 62 | * @param rows The rows to transform 63 | * @param field The field holds the reference point and coordinate systems 64 | * @return rows transformed by the transformation of the field's coordinates 65 | */ 66 | inline Rows removeRowsRefPoint( 67 | const Rows & rows, 68 | const F2CField & field) 69 | { 70 | Rows trans_rows; 71 | for (const auto & row : rows) { 72 | LineString new_row = row.first.clone(); 73 | for (auto & point : new_row) { 74 | point = point - field.getRefPoint(); 75 | } 76 | trans_rows.emplace_back(new_row, row.second); 77 | } 78 | return trans_rows; 79 | } 80 | 81 | /** 82 | * @brief Parse the rows from file format 83 | * @param file_path The path of the file to parse 84 | * @param reorder Whether to reorder by ID 85 | * @throw std::invalid_argument If the file is invalid 86 | * @return Rows A vector of line strings corresponding to the row 87 | */ 88 | inline Rows parseRows(const std::string & file_path, const bool reorder = false) 89 | { 90 | tinyxml2::XMLDocument doc; 91 | doc.LoadFile(file_path.c_str()); 92 | auto * p_parcel = doc.RootElement(); 93 | 94 | if (p_parcel == nullptr) { 95 | throw std::invalid_argument("File not found"); 96 | } 97 | 98 | Rows rows; 99 | for (tinyxml2::XMLElement * rowElement = p_parcel->FirstChildElement("Row"); rowElement; 100 | rowElement = rowElement->NextSiblingElement("Row")) 101 | { 102 | const char * id = rowElement->Attribute("id"); 103 | if (!id) { 104 | id = "0"; 105 | } 106 | 107 | tinyxml2::XMLElement * geometry = rowElement->FirstChildElement("geometry"); 108 | if (!geometry) { 109 | throw std::invalid_argument("Failed to get geometry"); 110 | } 111 | 112 | tinyxml2::XMLElement * line_string = geometry->FirstChildElement("gml:LineString"); 113 | if (!line_string) { 114 | throw std::invalid_argument("Failed to get line string"); 115 | } 116 | 117 | const char * srs_name = line_string->Attribute("srsName"); 118 | if (!srs_name) { 119 | throw std::invalid_argument("Failed to get srsName"); 120 | } 121 | 122 | tinyxml2::XMLElement * coords = line_string->FirstChildElement("gml:coordinates"); 123 | if (!coords) { 124 | throw std::invalid_argument("Failed to get coords"); 125 | } 126 | 127 | const char * coord_text = coords->GetText(); 128 | if (!coord_text) { 129 | throw std::invalid_argument("Failed to get coord text"); 130 | } 131 | std::string p_coords = std::string(coord_text); 132 | 133 | auto findAndReplaceAll = 134 | [](std::string & data, std::string toSearch, std::string replaceStr) { 135 | size_t pos = data.find(toSearch); 136 | while (pos != std::string::npos) { 137 | data.replace(pos, toSearch.size(), replaceStr); 138 | pos = data.find(toSearch, pos + replaceStr.size()); 139 | } 140 | return; 141 | }; 142 | 143 | findAndReplaceAll(p_coords, ",", ";"); 144 | findAndReplaceAll(p_coords, " ", ", "); 145 | findAndReplaceAll(p_coords, ";", " "); 146 | p_coords = "LINESTRING (" + p_coords + ")"; 147 | 148 | OGRGeometry * new_geom = nullptr; 149 | auto result = OGRGeometryFactory::createFromWkt(p_coords.c_str(), nullptr, &new_geom); 150 | 151 | if (result == OGRERR_NONE) { 152 | rows.emplace_back(new_geom, std::stoi(id)); 153 | } else { 154 | throw std::invalid_argument("Failed to create linestring from wkt"); 155 | } 156 | OGRGeometryFactory::destroyGeometry(new_geom); 157 | } 158 | 159 | if (reorder) { 160 | std::sort( 161 | rows.begin(), rows.end(), 162 | [](auto & left, auto & right) { 163 | return left.second < right.second; 164 | }); 165 | } 166 | 167 | return rows; 168 | } 169 | 170 | } // namespace util 171 | 172 | } // namespace opennav_row_coverage 173 | 174 | #endif // OPENNAV_ROW_COVERAGE__UTILS_HPP_ 175 | -------------------------------------------------------------------------------- /opennav_row_coverage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opennav_row_coverage 5 | 0.0.1 6 | A coverage server based on pre-defined rows 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | nav2_util 14 | fields2cover 15 | opennav_coverage 16 | opennav_coverage_msgs 17 | tinyxml2 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /opennav_row_coverage/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "opennav_row_coverage/row_coverage_server.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /opennav_row_coverage/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #Test Swath generation 2 | ament_add_gtest(test_swath_generator 3 | test_swath_generator.cpp 4 | ) 5 | ament_target_dependencies(test_swath_generator 6 | ${dependencies} 7 | ) 8 | target_link_libraries(test_swath_generator 9 | ${library_name} 10 | ) 11 | 12 | #Test utils 13 | ament_add_gtest(test_utils 14 | test_utils.cpp 15 | ) 16 | ament_target_dependencies(test_utils 17 | ${dependencies} 18 | ) 19 | target_link_libraries(test_utils 20 | ${library_name} 21 | ) 22 | -------------------------------------------------------------------------------- /opennav_row_coverage/test/test_server.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 18 | #include "opennav_coverage/coverage_server.hpp" 19 | #include "tf2/utils.h" 20 | #include "ament_index_cpp/get_package_share_directory.hpp" 21 | 22 | // Luckily, F2C has very high test coverage so we only need to test what we touch 23 | 24 | class RosLockGuard 25 | { 26 | public: 27 | RosLockGuard() {rclcpp::init(0, nullptr);} 28 | ~RosLockGuard() {rclcpp::shutdown();} 29 | }; 30 | RosLockGuard g_rclcpp; 31 | 32 | namespace opennav_coverage 33 | { 34 | 35 | class ServerShim : public opennav_row_coverage::RowCoverageServer 36 | { 37 | public: 38 | ServerShim() 39 | : RowCoverageServer() 40 | {} 41 | void configure(const rclcpp_lifecycle::State & state) 42 | { 43 | this->on_configure(state); 44 | cartesian_frame_ = false; // Test files in GPS 45 | } 46 | void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} 47 | void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} 48 | void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} 49 | void shutdown(const rclcpp_lifecycle::State & state) {this->on_shutdown(state);} 50 | 51 | bool validateGoalShim(typename std::shared_ptr req) 52 | { 53 | return validateGoal(req); 54 | } 55 | }; 56 | 57 | TEST(ServerTest, LifecycleTest) 58 | { 59 | auto node = std::make_shared(); 60 | rclcpp_lifecycle::State state; 61 | node->configure(state); 62 | node->activate(state); 63 | node->deactivate(state); 64 | node->cleanup(state); 65 | node->shutdown(state); 66 | node.reset(); 67 | } 68 | 69 | TEST(ServerTest, testUtils) 70 | { 71 | auto node = std::make_shared(); 72 | rclcpp_lifecycle::State state; 73 | node->configure(state); 74 | node->activate(state); 75 | 76 | auto req = std::make_shared(); 77 | EXPECT_TRUE(node->validateGoalShim(req)); 78 | req->generate_route = false; 79 | EXPECT_FALSE(node->validateGoalShim(req)); 80 | } 81 | 82 | TEST(ServerTest, testServerTransactions) 83 | { 84 | // Create server 85 | auto node = std::make_shared(); 86 | rclcpp_lifecycle::State state; 87 | node->configure(state); 88 | node->activate(state); 89 | auto node_thread = std::make_unique(node); 90 | 91 | // Send some requests 92 | auto client_node = std::make_shared("my_node"); 93 | auto action_client = 94 | rclcpp_action::create_client( 95 | client_node, "compute_coverage_path"); 96 | action_client->wait_for_action_server(); 97 | 98 | auto goal_msg = opennav_coverage_msgs::action::ComputeCoveragePath::Goal(); 99 | goal_msg.use_gml_file = true; // Use file 100 | goal_msg.gml_field = 101 | ament_index_cpp::get_package_share_directory("opennav_coverage") + "/irregular_test_field.xml"; 102 | 103 | auto future_goal_handle = action_client->async_send_goal(goal_msg); 104 | EXPECT_EQ( 105 | rclcpp::spin_until_future_complete( 106 | client_node, 107 | future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); 108 | 109 | auto goal_handle = future_goal_handle.get(); 110 | 111 | // Wait for the result 112 | auto future_result = action_client->async_get_result(goal_handle); 113 | EXPECT_EQ( 114 | rclcpp::spin_until_future_complete(client_node, future_result), 115 | rclcpp::FutureReturnCode::SUCCESS); 116 | 117 | // The final result 118 | auto result = future_result.get(); 119 | EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); 120 | } 121 | 122 | TEST(ServerTest, testDynamicParams) 123 | { 124 | auto node = std::make_shared(); 125 | rclcpp_lifecycle::State state; 126 | node->configure(state); 127 | node->activate(state); 128 | 129 | auto rec_param = std::make_shared( 130 | node->get_node_base_interface(), node->get_node_topics_interface(), 131 | node->get_node_graph_interface(), 132 | node->get_node_services_interface()); 133 | 134 | auto results = rec_param->set_parameters_atomically( 135 | {rclcpp::Parameter("default_turn_point_distance", 0.25), 136 | rclcpp::Parameter("robot_width", 1.0), 137 | rclcpp::Parameter("operation_width", 1.12), 138 | rclcpp::Parameter("default_path_type", std::string("hi")), 139 | rclcpp::Parameter("default_path_continuity_type", std::string("hi")), 140 | rclcpp::Parameter("default_route_type", std::string("hi")), 141 | rclcpp::Parameter("default_swath_type", std::string("hi")), 142 | rclcpp::Parameter("default_offset", 0.1), 143 | rclcpp::Parameter("default_spiral_n", 41), 144 | rclcpp::Parameter("coordinates_in_cartesian_frame", false), 145 | rclcpp::Parameter("default_custom_order", std::vector{1, 2, 3})}); 146 | 147 | rclcpp::spin_until_future_complete( 148 | node->get_node_base_interface(), 149 | results); 150 | 151 | EXPECT_EQ(node->get_parameter("default_swath_type").as_string(), std::string("hi")); 152 | EXPECT_EQ(node->get_parameter("default_spiral_n").as_int(), 41); 153 | EXPECT_EQ(node->get_parameter("coordinates_in_cartesian_frame").as_bool(), false); 154 | } 155 | 156 | } // namespace opennav_coverage 157 | -------------------------------------------------------------------------------- /opennav_row_coverage/test/test_swath_generator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | #include "opennav_row_coverage/utils.hpp" 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "opennav_row_coverage/row_swath_generator.hpp" 19 | #include "opennav_coverage_msgs/msg/row_swath_mode.hpp" 20 | #include "ament_index_cpp/get_package_share_directory.hpp" 21 | 22 | class RosLockGuard 23 | { 24 | public: 25 | RosLockGuard() {rclcpp::init(0, nullptr);} 26 | ~RosLockGuard() {rclcpp::shutdown();} 27 | }; 28 | RosLockGuard g_rclcpp; 29 | 30 | namespace opennav_row_coverage 31 | { 32 | 33 | class SwathTestFixture : public ::testing::Test 34 | { 35 | public: 36 | void SetUp() 37 | { 38 | node_ = std::make_unique("test_node"); 39 | generator_ = std::make_unique(node_); 40 | 41 | const std::string file_path = 42 | ament_index_cpp::get_package_share_directory("opennav_coverage") + 43 | "/cartesian_test_field.xml"; 44 | rows_ = opennav_row_coverage::util::parseRows(file_path); 45 | } 46 | 47 | protected: 48 | rclcpp::Node::SharedPtr node_; 49 | std::unique_ptr generator_; 50 | F2CField field_; 51 | Rows rows_; 52 | }; 53 | 54 | TEST_F(SwathTestFixture, TestCenter) 55 | { 56 | opennav_coverage_msgs::msg::RowSwathMode settings; 57 | settings.mode = "CENTER"; 58 | 59 | auto swaths = generator_->generateSwaths(rows_, settings); 60 | 61 | ASSERT_EQ(swaths.size(), 6); 62 | EXPECT_EQ(swaths[0].getId(), 1); 63 | EXPECT_EQ(swaths[0].getPath().getX(0), 6.475); 64 | EXPECT_EQ(swaths[0].getPath().getY(0), 5.0); 65 | EXPECT_EQ(swaths[0].getPath().getX(1), 6.13); 66 | EXPECT_EQ(swaths[0].getPath().getY(1), 15.0); 67 | } 68 | 69 | TEST_F(SwathTestFixture, TestCenterWithSkipping) 70 | { 71 | opennav_coverage_msgs::msg::RowSwathMode settings; 72 | settings.mode = "CENTER"; 73 | settings.skip_ids = {1, 10}; 74 | 75 | auto swaths = generator_->generateSwaths(rows_, settings); 76 | ASSERT_EQ(swaths.size(), 5); 77 | EXPECT_EQ(swaths[0].getId(), 2); 78 | EXPECT_NEAR(swaths[0].getPath().getX(0), 8.04, 1e-3); 79 | EXPECT_EQ(swaths[0].getPath().getY(0), 5.0); 80 | EXPECT_NEAR(swaths[0].getPath().getX(1), 8.03, 1e-3); 81 | EXPECT_EQ(swaths[0].getPath().getY(1), 15.0); 82 | } 83 | 84 | TEST_F(SwathTestFixture, TestOffset) 85 | { 86 | opennav_coverage_msgs::msg::RowSwathMode settings; 87 | settings.mode = "OFFSET"; 88 | settings.offset = 6.0; 89 | settings.skip_ids = {1, 10}; 90 | 91 | auto swaths = generator_->generateSwaths(rows_, settings); 92 | 93 | ASSERT_EQ(swaths.size(), 5); 94 | EXPECT_EQ(swaths[0].getId(), 2); 95 | EXPECT_NEAR(swaths[0].getPath().getX(0), 13.699, 1e-3); 96 | EXPECT_EQ(swaths[0].getPath().getY(0), 5.0); 97 | EXPECT_NEAR(swaths[0].getPath().getX(1), 12.76, 1e-3); 98 | EXPECT_EQ(swaths[0].getPath().getY(1), 15.0); 99 | } 100 | 101 | TEST_F(SwathTestFixture, TestRowsAreSwaths) 102 | { 103 | opennav_coverage_msgs::msg::RowSwathMode settings; 104 | settings.mode = "ROWSARESWATHS"; 105 | settings.skip_ids = {1, 2}; 106 | 107 | auto swaths = generator_->generateSwaths(rows_, settings); 108 | 109 | EXPECT_EQ(swaths.size(), 5); 110 | 111 | EXPECT_NEAR(swaths[0].getPath().getX(0), rows_[2].first.getX(0), 1e-3); 112 | EXPECT_NEAR(swaths[0].getPath().getY(0), rows_[2].first.getY(0), 1e-3); 113 | 114 | EXPECT_NEAR(swaths[0].getPath().getX(1), rows_[2].first.getX(1), 1e-3); 115 | EXPECT_NEAR(swaths[0].getPath().getY(1), rows_[2].first.getY(1), 1e-3); 116 | EXPECT_NEAR(swaths[0].getWidth(), 1.0699, 1e-3); 117 | 118 | EXPECT_EQ(swaths[0].getId(), 3); 119 | } 120 | 121 | } // namespace opennav_row_coverage 122 | -------------------------------------------------------------------------------- /opennav_row_coverage/test/test_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Open Navigation LLC 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "gtest/gtest.h" 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "opennav_row_coverage/utils.hpp" 20 | #include "tf2/utils.h" 21 | #include "ament_index_cpp/get_package_share_directory.hpp" 22 | 23 | class RosLockGuard 24 | { 25 | public: 26 | RosLockGuard() {rclcpp::init(0, nullptr);} 27 | ~RosLockGuard() {rclcpp::shutdown();} 28 | }; 29 | RosLockGuard g_rclcpp; 30 | 31 | namespace opennav_row_coverage 32 | { 33 | 34 | TEST(UtilsTests, TestRowParserSimple) 35 | { 36 | const std::string file_path = 37 | ament_index_cpp::get_package_share_directory("opennav_coverage") + 38 | "/cartesian_test_field.xml"; 39 | auto rows = opennav_row_coverage::util::parseRows(file_path); 40 | ASSERT_EQ(rows.size(), 7); 41 | } 42 | 43 | TEST(UtilsTests, TestRowParserComplex) 44 | { 45 | const std::string file_path = 46 | ament_index_cpp::get_package_share_directory("opennav_coverage") + 47 | "/irregular_test_field.xml"; 48 | auto rows = opennav_row_coverage::util::parseRows(file_path); 49 | 50 | ASSERT_EQ(rows.size(), 10); 51 | 52 | EXPECT_EQ(rows[2].first.getX(0), 4.26122335903712); 53 | EXPECT_EQ(rows[2].first.getY(0), 51.7859542010475); 54 | EXPECT_EQ(rows[2].second, 3); 55 | 56 | EXPECT_EQ(rows[4].first.getX(1), 4.25994305603656); 57 | EXPECT_EQ(rows[4].first.getY(1), 51.7899570320785); 58 | EXPECT_EQ(rows[4].second, 5); 59 | 60 | EXPECT_EQ(rows[8].first.getX(1), 4.25751883150527); 61 | EXPECT_EQ(rows[8].first.getY(1), 51.7903779047924); 62 | EXPECT_EQ(rows[8].second, 9); 63 | 64 | EXPECT_EQ(rows[9].first.getX(0), 4.25644928223945); 65 | EXPECT_EQ(rows[9].first.getY(0), 51.7894578087076); 66 | EXPECT_EQ(rows[9].second, 10); 67 | } 68 | 69 | } // namespace opennav_row_coverage 70 | -------------------------------------------------------------------------------- /opennav_row_coverage/test/tester.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # Copyright 2023 Open Navigation LLC 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from enum import Enum 17 | 18 | from action_msgs.msg import GoalStatus 19 | from ament_index_python.packages import get_package_share_directory 20 | from lifecycle_msgs.srv import ChangeState, GetState 21 | from opennav_coverage_msgs.action import ComputeCoveragePath 22 | from opennav_coverage_msgs.msg import Coordinate # Coordinates 23 | import rclpy 24 | from rclpy.action import ActionClient 25 | from rclpy.node import Node 26 | 27 | 28 | class TaskResult(Enum): 29 | UNKNOWN = 0 30 | SUCCEEDED = 1 31 | CANCELED = 2 32 | FAILED = 3 33 | 34 | 35 | class CoverageTester(Node): 36 | 37 | def __init__(self): 38 | super().__init__(node_name='coverage_tester') 39 | self.goal_handle = None 40 | self.result_future = None 41 | self.status = None 42 | 43 | self.coverage_client = ActionClient(self, ComputeCoveragePath, 44 | 'compute_coverage_path') 45 | 46 | def destroy_node(self): 47 | self.coverage_client.destroy() 48 | super().destroy_node() 49 | 50 | def getCoveragePath(self, goal_msg): 51 | """Send a `ComputeCoveragePath` action request.""" 52 | # self.debug("Waiting for 'ComputeCoveragePath' action server") 53 | while not self.coverage_client.wait_for_server(timeout_sec=1.0): 54 | # self.info("'ComputeCoveragePath' action server not available, waiting...") 55 | pass 56 | 57 | # self.info(f'Computing coverage path....') 58 | send_goal_future = self.coverage_client.send_goal_async(goal_msg) 59 | rclpy.spin_until_future_complete(self, send_goal_future) 60 | self.goal_handle = send_goal_future.result() 61 | 62 | if not self.goal_handle.accepted: 63 | # self.error('Get coverage path was rejected!') 64 | return None 65 | 66 | self.result_future = self.goal_handle.get_result_async() 67 | rclpy.spin_until_future_complete(self, self.result_future) 68 | self.status = self.result_future.result().status 69 | if self.status != GoalStatus.STATUS_SUCCEEDED: 70 | # self.warn(f'Getting path failed with status code: {self.status}') 71 | return None 72 | 73 | return self.result_future.result().result 74 | 75 | def getResult(self): 76 | """Get the pending action result message.""" 77 | if self.status == GoalStatus.STATUS_SUCCEEDED: 78 | return TaskResult.SUCCEEDED 79 | elif self.status == GoalStatus.STATUS_ABORTED: 80 | return TaskResult.FAILED 81 | elif self.status == GoalStatus.STATUS_CANCELED: 82 | return TaskResult.CANCELED 83 | else: 84 | return TaskResult.UNKNOWN 85 | 86 | def startup(self): 87 | """Activate coverage server.""" 88 | change_state_client = self.create_client( 89 | GetState, 'row_coverage_server/get_state') 90 | req = GetState.Request() 91 | fut = change_state_client.call_async(req) 92 | rclpy.spin_until_future_complete(self, fut) 93 | if fut.result().current_state.id == 3: 94 | return 95 | 96 | change_state_client = self.create_client( 97 | ChangeState, 'row_coverage_server/change_state') 98 | req = ChangeState.Request() 99 | req.transition.id = 1 100 | fut = change_state_client.call_async(req) 101 | rclpy.spin_until_future_complete(self, fut) 102 | req.transition.id = 3 103 | fut = change_state_client.call_async(req) 104 | rclpy.spin_until_future_complete(self, fut) 105 | 106 | 107 | def toMsg(val): 108 | coord = Coordinate() 109 | coord.axis1 = val[0] 110 | coord.axis2 = val[1] 111 | return coord 112 | 113 | 114 | def main(): 115 | rclpy.init() 116 | 117 | tester = CoverageTester() 118 | tester.startup() 119 | 120 | goal = ComputeCoveragePath.Goal() 121 | 122 | # Populate different setting combinations 123 | goal.generate_headland = True 124 | goal.generate_route = True 125 | goal.generate_path = True 126 | 127 | goal.use_gml_file = True 128 | goal.gml_field = get_package_share_directory('opennav_coverage') + '/irregular_test_field.xml' 129 | 130 | # goal.row_swath_mode.mode = "CENTER" # OFFSET 131 | # goal.row_swath_mode.offset = 0.5 132 | 133 | # goal.route_mode.mode = "SPIRAL" # BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM 134 | # goal.route_mode.spiral_n = 3 135 | # goal.route_mode.custom_order 136 | 137 | # goal.path_mode.mode = "DUBIN" # DUBIN, REEDS_SHEPP 138 | # goal.path_mode.continuity_mode = "CONTINUOUS" # CONTINUOUS, DISCONTINUOUS 139 | # goal.path_mode.turn_point_distance = 0.1 140 | 141 | coverage_info = tester.getCoveragePath(goal) 142 | result = tester.getResult() 143 | if result == TaskResult.SUCCEEDED: 144 | print('Got coverage path!') 145 | elif result == TaskResult.CANCELED: 146 | print('Coverage path request was canceled!') 147 | elif result == TaskResult.FAILED: 148 | print('Failed to obtain coverage path!') 149 | else: 150 | print('Goal has an invalid return status!') 151 | 152 | if coverage_info is not None: 153 | print(f'Coverage path has {len(coverage_info.coverage_path.swaths)} swaths') 154 | print(f'Coverage path has {len(coverage_info.coverage_path.turns)} turns') 155 | print(f'Coverage path has {len(coverage_info.nav_path.poses)} nav path poses') 156 | 157 | print('exiting') 158 | exit(0) 159 | 160 | 161 | if __name__ == '__main__': 162 | main() 163 | --------------------------------------------------------------------------------