diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index d3c41654..16256309 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -1,7 +1,7 @@ repositories: ThirdParty/EasyNavigation: type: git - url: https://github.com/EasyNavigation/EasyNavigation.git + url: https://github.com/juanscelyg/EasyNavigation.git version: humble ThirdParty/yaets: type: git @@ -9,5 +9,5 @@ repositories: version: humble ThirdParty/NavMap: type: git - url: https://github.com/EasyNavigation/NavMap.git + url: https://github.com/juanscelyg/NavMap.git version: humble diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 1bc35f70..f805b91e 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -7,33 +7,24 @@ on: push: branches: - humble - schedule: - - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy steps: - name: Repo checkout uses: actions/checkout@v4 - with: - ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: humble - - name: Upgrade typing_extensions - run: python3 -m pip install --upgrade "typing_extensions>=4.7" - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mpc_controller easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_octomap_maps_manager easynav_regulated_pp_controller easynav_routes_maps_manager easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller target-ros2-distro: humble - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + vcs-repo-file-url: ${{ github.workspace }}/.github/thirdparty.repos + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 33198952..89695fac 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -7,15 +7,12 @@ on: push: branches: - jazzy - schedule: - - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - name: Repo checkout uses: actions/checkout@v4 @@ -23,16 +20,13 @@ jobs: ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: jazzy - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml new file mode 100644 index 00000000..4fbccdb1 --- /dev/null +++ b/.github/workflows/jazzy_cron.yaml @@ -0,0 +1,44 @@ +name: jazzy + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: jazzy + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: jazzy + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index f699529f..ee4356be 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -7,15 +7,12 @@ on: push: branches: - kilted - schedule: - - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - name: Repo checkout uses: actions/checkout@v4 @@ -23,16 +20,12 @@ jobs: ref: kilted - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: kilted - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: kilted - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml new file mode 100644 index 00000000..7a245495 --- /dev/null +++ b/.github/workflows/kilted_cron.yaml @@ -0,0 +1,44 @@ +name: kilted + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: kilted + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: kilted + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/lyrical.yaml b/.github/workflows/lyrical.yaml new file mode 100644 index 00000000..8443a2fb --- /dev/null +++ b/.github/workflows/lyrical.yaml @@ -0,0 +1,49 @@ +name: lyrical + +on: + pull_request: + branches: + - lyrical + push: + branches: + - lyrical + schedule: + - cron: '0 0 * * 6' + workflow_dispatch: +jobs: + build-and-test: + runs-on: ubuntu-26.04 + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-lyrical-ros-base:master + steps: + - name: Repo checkout + uses: actions/checkout@v6 + with: + ref: lyrical + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.8 + with: + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: lyrical + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 30cd778d..cafefba5 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -8,31 +8,25 @@ on: branches: - rolling schedule: - - cron: '0 0 * * 6' + - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-26.04 + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master steps: - name: Repo checkout - uses: actions/checkout@v4 + uses: actions/checkout@v6 with: ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: rolling - - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 + uses: ros-tooling/action-ros-ci@0.4.8 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..46e553ed --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +# VS Code stuff +/.vscode/** +**/__pycache__/ + +# ROS 2 build files +build/ +install/ +log/ diff --git a/LICENSE b/LICENSE index f288702d..261eeb9e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,674 +1,201 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md index 1f2c8957..15cc761c 100644 --- a/README.md +++ b/README.md @@ -2,11 +2,15 @@ [![Doxygen Deployment](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/doxygen-doc.yml/badge.svg)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/doxygen-doc.yml) [![rolling](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml/badge.svg?branch=rolling)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml) +[![lyrical](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/lyrical.yaml/badge.svg?branch=lyrical)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/lyrical.yaml) [![kilted](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/kilted.yaml/badge.svg?branch=kilted)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/kilted.yaml) [![jazzy](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/jazzy.yaml/badge.svg?branch=jazzy)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/jazzy.yaml) [![humble](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/humble.yaml/badge.svg?branch=humble)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/humble.yaml) +📋 Roadmap Project: [RoadMap](https://github.com/EasyNavigation/EasyNavigation/blob/rolling/ROADMAP.md) + ## Description + **EasyNav Plugins** provides the official collection of plugins for the [Easy Navigation (EasyNav)](https://github.com/EasyNavigation) framework. These plugins extend the navigation core with planners, controllers, map managers, and localizers compatible with ROS 2. @@ -17,6 +21,7 @@ Each plugin resides in its own ROS 2 package and is registered via `pluginlib`, ## Repository Structure ### 🧭 Planners + Path planning plugins implementing A*, costmap, or NavMap–based methods. | Package | Description | Link | @@ -28,6 +33,7 @@ Path planning plugins implementing A*, costmap, or NavMap–based methods. --- ### ⚙️ Controllers + Motion controllers for trajectory tracking and reactive behaviors. | Package | Description | Link | @@ -36,10 +42,13 @@ Motion controllers for trajectory tracking and reactive behaviors. | `easynav_mppi_controller` | Model Predictive Path Integral (MPPI) controller. | [README](./controllers/easynav_mppi_controller/README.md) | | `easynav_simple_controller` | Simple proportional controller for testing. | [README](./controllers/easynav_simple_controller/README.md) | | `easynav_serest_controller` | SeReST (Safe Reactive Steering) controller. | [README](./controllers/easynav_serest_controller/README.md) | +| `easynav_mpc_controller` | Model Predictive Controller (MPC). | [README](./controllers/easynav_mpc_controller/README.md) | +| `easynav_regulated_pp_controller` | Regulated Pure Pursuit controller, with optional Dynamic Window (DWPP) extension. | [README](./controllers/easynav_regulated_pp_controller/README.md) | --- ### 🗺️ Maps Managers + Map management plugins that provide, update, and store different environment representations. | Package | Description | Link | @@ -53,6 +62,7 @@ Map management plugins that provide, update, and store different environment rep --- ### 📍 Localizers + Localization plugins based on different map types and sensors. | Package | Description | Link | @@ -61,9 +71,10 @@ Localization plugins based on different map types and sensors. | `easynav_simple_localizer` | Basic localizer for SimpleMap–based setups. | [README](./localizers/easynav_simple_localizer/README.md) | | `easynav_navmap_localizer` | AMCL-like localizer operating on NavMap meshes. | [README](./localizers/easynav_navmap_localizer/README.md) | | `easynav_costmap_localizer` | AMCL-like localizer using Costmap2D. | [README](./localizers/easynav_costmap_localizer/README.md) | +| `easynav_fusion_localizer` | Multi-sensor fusion localizer (e.g., GPS + odometry + map). | [README](./localizers/easynav_fusion_localizer/README.md) | --- ## License -All packages in this repository are released under **GPL-3.0-only** unless stated otherwise in the individual package. +All packages in this repository are released under **Apache-2.0** unless stated otherwise in the individual package. diff --git a/common/easynav_costmap_common/CHANGELOG.rst b/common/easynav_costmap_common/CHANGELOG.rst index 924b909c..5b9c3369 100644 --- a/common/easynav_costmap_common/CHANGELOG.rst +++ b/common/easynav_costmap_common/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package easynav_costmap_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Any change in the costmap set it as modified +* Adding timestamp to costmap +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Remove C++20/C++23 features and update to new MethodBase interface +* Cleanup unused headers +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/common/easynav_costmap_common/CMakeLists.txt b/common/easynav_costmap_common/CMakeLists.txt index fd5578d7..de103729 100644 --- a/common/easynav_costmap_common/CMakeLists.txt +++ b/common/easynav_costmap_common/CMakeLists.txt @@ -12,6 +12,9 @@ set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -24,6 +27,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common ${nav_msgs_TARGETS} + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs ) install( @@ -56,5 +62,8 @@ ament_export_dependencies( easynav_common # pluginlib nav_msgs + rclcpp + geometry_msgs + tf2_geometry_msgs ) ament_package() diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp index d141f071..e0750d4a 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp @@ -38,15 +38,13 @@ #ifndef EASYNAV_COSTMAP_COMMON__COSTMAP_2D_HPP_ #define EASYNAV_COSTMAP_COMMON__COSTMAP_2D_HPP_ -#include -#include -#include +#include #include #include -#include +#include #include -#include #include +#include "rclcpp/time.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -129,10 +127,29 @@ class Costmap2D * * The resulting OccupancyGrid message contains metadata such as resolution, size, and origin, * and its data field is populated with cost values from the costmap. Cells with a value of - * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly. + * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly. + * + * The output message header stamp is set to the internal last-modified timestamp. */ void toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const; + /** + * @brief Get timestamp of last costmap modification. + * + * Set by constructors and copy/assignment, and may be updated by some mutating operations. + * For high-frequency per-cell updates (e.g., many `setCost()` calls in a loop), prefer calling + * `touch()` once after the batch update. + */ + rclcpp::Time getLastModifiedStamp() const; + + /** + * @brief Mark the costmap as modified. + * + * This bumps the internal last-modified stamp by 1 nanosecond while preserving the clock type. + * It is intended to be called after bulk updates to avoid per-cell overhead. + */ + void touch(); + /** * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap @param source Source costmap where the window will be copied from @@ -583,6 +600,10 @@ class Costmap2D unsigned char * costmap_; unsigned char default_value_; + // Timestamp indicating when the costmap was last modified. + // Set by constructors and copy/assignment, and bumped by some mutating operations. + rclcpp::Time last_modified_; + // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels class MarkCell { diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp index e7f4b640..744f724a 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp @@ -16,7 +16,6 @@ #define EASYNAV_COSTMAP_COMMON__GEOMETRY_UTILS_HPP_ #include -#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp index a563202e..9834ef89 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp @@ -35,6 +35,7 @@ #ifndef EASYNAV_COSTMAP_COMMON__OCC_GRID_VALUES_HPP_ #define EASYNAV_COSTMAP_COMMON__OCC_GRID_VALUES_HPP_ +#include namespace easynav { diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 55db3489..cf01089e 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,7 +2,7 @@ easynav_costmap_common - 0.0.2 + 0.4.0 Easy Navigation: Simple Common package. Francisco Martín Rico @@ -11,7 +11,7 @@ Juan Sebastián Cely Gutiérrez José Miguel Guerrero Hernández - GPL-3.0-only + Apache-2.0 Francisco Martín Rico Francisco Miguel Moreno Olivo @@ -21,7 +21,10 @@ easynav_common nav_msgs + rclcpp ament_index_cpp + geometry_msgs + tf2_geometry_msgs ament_lint_auto ament_lint_common diff --git a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp index aeb8b088..1b7a5f14 100644 --- a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp +++ b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp @@ -39,11 +39,7 @@ #include #include -#include #include -#include -#include -#include #include "easynav_costmap_common/cost_values.hpp" #include "easynav_costmap_common/occ_grid_values.hpp" @@ -55,7 +51,7 @@ Costmap2D::Costmap2D( unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value) : resolution_(resolution), origin_x_(origin_x), - origin_y_(origin_y), costmap_(NULL), default_value_(default_value) + origin_y_(origin_y), costmap_(NULL), default_value_(default_value), last_modified_(0) { access_ = new mutex_t(); @@ -65,7 +61,7 @@ Costmap2D::Costmap2D( } Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) -: default_value_(FREE_SPACE) +: default_value_(FREE_SPACE), last_modified_(map.header.stamp) { access_ = new mutex_t(); @@ -130,6 +126,9 @@ void Costmap2D::resetMaps() { std::unique_lock lock(*access_); memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); + + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn) @@ -145,6 +144,9 @@ void Costmap2D::resetMapToValue( for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) { memset(costmap_ + y, value, len * sizeof(unsigned char)); } + + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } bool Costmap2D::copyCostmapWindow( @@ -183,6 +185,11 @@ bool Costmap2D::copyCostmapWindow( map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_); + + { + std::unique_lock lock(*access_); + last_modified_ = map.getLastModifiedStamp(); + } return true; } @@ -206,6 +213,15 @@ bool Costmap2D::copyWindow( source.costmap_, sx0, sy0, source.size_x_, costmap_, dx0, dy0, size_x_, sz_x, sz_y); + + { + const auto source_stamp = source.getLastModifiedStamp(); + std::unique_lock lock(*access_); + const int64_t new_stamp_ns = std::max( + last_modified_.nanoseconds(), + source_stamp.nanoseconds()) + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); + } return true; } @@ -214,6 +230,10 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const { std::lock_guard lock(*access_); + const int64_t stamp_ns = last_modified_.nanoseconds(); + msg.header.stamp.sec = static_cast(stamp_ns / 1000000000LL); + msg.header.stamp.nanosec = static_cast(stamp_ns % 1000000000LL); + msg.info.width = size_x_; msg.info.height = size_y_; msg.info.resolution = resolution_; @@ -235,6 +255,19 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const } } +rclcpp::Time Costmap2D::getLastModifiedStamp() const +{ + std::lock_guard lock(*access_); + return last_modified_; +} + +void Costmap2D::touch() +{ + std::lock_guard lock(*access_); + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); +} + Costmap2D & Costmap2D::operator=(const Costmap2D & map) { // check for self assignment @@ -251,6 +284,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) origin_x_ = map.origin_x_; origin_y_ = map.origin_y_; default_value_ = map.default_value_; + last_modified_ = map.last_modified_; // initialize our various maps initMaps(size_x_, size_y_); @@ -262,7 +296,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) } Costmap2D::Costmap2D(const Costmap2D & map) -: costmap_(NULL) +: costmap_(NULL), last_modified_(0) { access_ = new mutex_t(); *this = map; @@ -270,7 +304,8 @@ Costmap2D::Costmap2D(const Costmap2D & map) // just initialize everything to NULL by default Costmap2D::Costmap2D() -: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) +: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), + costmap_(NULL), last_modified_(0) { access_ = new mutex_t(); } @@ -309,7 +344,16 @@ unsigned char Costmap2D::getCost(unsigned int undex) const void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { - costmap_[getIndex(mx, my)] = cost; + std::lock_guard lock(*access_); + const unsigned int index = getIndex(mx, my); + + if (costmap_[index] == cost) { + return; + } + + costmap_[index] = cost; + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 70e969d9..81983326 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -1,26 +1,22 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_costmap_common/cost_values.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/time.hpp" using easynav::Costmap2D; @@ -121,8 +117,88 @@ TEST_F(Costmap2DTest, OccupancyGridConversion) }; for (size_t i = 0; i < grid.data.size(); ++i) { - bool expected = std::find(expected_indices.begin(), + bool expected = std::find( + expected_indices.begin(), expected_indices.end(), i) != expected_indices.end(); EXPECT_EQ(grid.data[i], expected ? 100 : 0); } } + +TEST_F(Costmap2DTest, TimestampFromOccupancyGridConstructor) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 123; + in.header.stamp.nanosec = 456u; + + in.info.width = 2u; + in.info.height = 3u; + in.info.resolution = 0.5; + in.info.origin.position.x = 1.0; + in.info.origin.position.y = -2.0; + in.data.assign(in.info.width * in.info.height, 0); + + Costmap2D map(in); + const int64_t expected_ns = 123LL * 1000000000LL + 456LL; + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + map.toOccupancyGridMsg(out); + + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} + +TEST_F(Costmap2DTest, TimestampFromCopyConstructor) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 10; + in.header.stamp.nanosec = 20u; + in.info.width = 1u; + in.info.height = 1u; + in.info.resolution = 1.0; + in.info.origin.position.x = 0.0; + in.info.origin.position.y = 0.0; + in.data.assign(1u, 0); + + Costmap2D original(in); + Costmap2D copy(original); + + const int64_t expected_ns = 10LL * 1000000000LL + 20LL; + EXPECT_EQ(copy.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + copy.toOccupancyGridMsg(out); + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} + +TEST_F(Costmap2DTest, TimestampUpdatedBySetCost) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 7; + in.header.stamp.nanosec = 8u; + in.info.width = 3u; + in.info.height = 3u; + in.info.resolution = 1.0; + in.info.origin.position.x = 0.0; + in.info.origin.position.y = 0.0; + in.data.assign(in.info.width * in.info.height, 0); + + Costmap2D map(in); + const int64_t expected_ns = 7LL * 1000000000LL + 8LL; + + map.setCost(1, 1, 100); + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1); + + // Setting the same value should not change the stamp. + map.setCost(1, 1, 100); + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1); + + nav_msgs::msg::OccupancyGrid out; + map.toOccupancyGridMsg(out); + + const int64_t out_ns = + static_cast(out.header.stamp.sec) * 1000000000LL + + static_cast(out.header.stamp.nanosec); + EXPECT_EQ(out_ns, expected_ns + 1); +} diff --git a/common/easynav_simple_common/CHANGELOG.rst b/common/easynav_simple_common/CHANGELOG.rst index 7d5ee0d0..715de44e 100644 --- a/common/easynav_simple_common/CHANGELOG.rst +++ b/common/easynav_simple_common/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package easynav_simple_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Sync with Rolling branch +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Rolling updates +* TFInfo in RTTFBuffer +* Fix downsample in Simple +* Cleanup unused headers +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp index 331a820c..4039c221 100644 --- a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp +++ b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the SimpleMap type. @@ -24,11 +20,7 @@ #define EASYNAV_PLANNER__SIMPLEMAP_HPP_ #include -#include -#include #include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 63b5bae1..0838a159 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -2,10 +2,10 @@ easynav_simple_common - 0.0.2 + 0.4.0 Easy Navigation: Simple Common package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp index e469b9c1..e52eeaec 100644 --- a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp +++ b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp @@ -1,22 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include @@ -296,7 +291,8 @@ SimpleMap::downsample_factor(int factor) const } } - new_map->at(x, y) = (occupied_count > (factor * factor / 2)); + // Mark cell as occupied if any source cell in the block is occupied + new_map->at(x, y) = (occupied_count > 0); } } diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index d3f97cb6..b51b2d2b 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -1,28 +1,22 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include "easynav_simple_common/SimpleMap.hpp" -#include - /// \brief Fixture for SimpleMap tests (minimal) class SimpleMapTest : public ::testing::Test { @@ -127,7 +121,8 @@ TEST_F(SimpleMapTest, OccupancyGridConversion) }; for (std::size_t i = 0; i < grid_msg.data.size(); ++i) { - bool is_occupied = std::find(expected_indices.begin(), + bool is_occupied = std::find( + expected_indices.begin(), expected_indices.end(), i) != expected_indices.end(); EXPECT_EQ(grid_msg.data[i], is_occupied ? 100 : 0); } @@ -174,7 +169,7 @@ TEST_F(SimpleMapTest, DownsampleIntegerFactor) EXPECT_TRUE(downsampled->at(0, 0)); EXPECT_FALSE(downsampled->at(1, 0)); EXPECT_FALSE(downsampled->at(0, 1)); - EXPECT_FALSE(downsampled->at(1, 1)); + EXPECT_TRUE(downsampled->at(1, 1)); } /// \brief Downsampling to new resolution @@ -197,9 +192,9 @@ TEST_F(SimpleMapTest, DownsampleToResolution) EXPECT_DOUBLE_EQ(downsampled->origin_x(), -1.0); EXPECT_DOUBLE_EQ(downsampled->origin_y(), -1.0); - EXPECT_FALSE(downsampled->at(0, 0)); - EXPECT_FALSE(downsampled->at(1, 1)); - EXPECT_FALSE(downsampled->at(2, 2)); + EXPECT_TRUE(downsampled->at(0, 0)); + EXPECT_TRUE(downsampled->at(1, 1)); + EXPECT_TRUE(downsampled->at(2, 2)); } /// \brief Downsample that results in cropped edges diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst new file mode 100644 index 00000000..b77f4bbc --- /dev/null +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -0,0 +1,60 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_mpc_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +* Add missing easynav_sensors deps +* Add missing easynav_sensors deps +* Sync with Rolling branch +* Sync with rolling branch +* Merge pull request `#61 `_ from Butakus/rolling + Update plugins to new sensors API +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* 0.3.1 +* update changelogs +* Merge branch 'rolling' of github.com:midemig/easynav_plugins into rolling +* Merge branch 'EasyNavigation:rolling' into rolling +* Merge pull request `#51 `_ from EasyNavigation/license-kilted + GPLv3 -> Apache 2.0 +* GPLv3 -> Apache 2.0 +* Merge pull request `#50 `_ from EasyNavigation/license + GPLv3 -> Apache 2.0 +* Merge pull request `#49 `_ from juanscelyg/rolling + Latest Updates and Corrected CI +* Merge pull request `#48 `_ from juanscelyg/kilted + Kilted +* GPLv3 -> Apache 2.0 +* Latest changes were added +* Dependencies were corrected +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.3.1 (2026-02-27) +------------------ +* GPLv3 -> Apache 2.0 +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Trim path for MPC +* Stop controllers at IDLE +* Obstacles are detected +* Methods were added to access to private attributes +* Optimizer header file was added +* Dependencies were fixed +* MPC with differential model is working +* MPC Controller minimizes path following +* Visualization Marker was added +* CI updated +* MPC controller is working +* Optimizer was changed +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel + +0.0.2 (2025-10-12) +------------------ diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt new file mode 100644 index 00000000..a8ff1d0b --- /dev/null +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_mpc_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_system REQUIRED) +find_package(easynav_sensors REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(PCL REQUIRED COMPONENTS common io) +find_package(pcl_conversions REQUIRED) +find_package(NLopt QUIET) +if(NOT NLopt_FOUND) + find_path(NLOPT_INCLUDE_DIR nlopt.hpp) + find_library(NLOPT_LIBRARY nlopt) + if(NLOPT_INCLUDE_DIR AND NLOPT_LIBRARY) + set(NLopt_FOUND TRUE) + set(NLopt_INCLUDE_DIRS ${NLOPT_INCLUDE_DIR}) + set(NLopt_LIBRARIES ${NLOPT_LIBRARY}) + endif() +endif() +if(NOT NLopt_FOUND) + message(FATAL_ERROR "NLopt not found. Install libnlopt-dev or set NLopt_DIR/CMAKE_PREFIX_PATH.") +endif() + +add_library(${PROJECT_NAME} SHARED + src/easynav_mpc_controller/MPCController.cpp + src/easynav_mpc_controller/MPCOptimizer.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + easynav_system::easynav_system + easynav_sensors::easynav_sensors + tf2_ros::tf2_ros + tf2::tf2 + pluginlib::pluginlib + Eigen3::Eigen + nlopt + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${PCL_LIBRARIES} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + # add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the planning plugins +pluginlib_export_plugin_description_file(easynav_core easynav_mpc_controller_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + easynav_sensors + pluginlib + tf2_ros + tf2 + geometry_msgs + nav_msgs + sensor_msgs + pcl_conversions + eigen + nlopt +) + +ament_package() diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md new file mode 100644 index 00000000..9d6fbbb3 --- /dev/null +++ b/controllers/easynav_mpc_controller/README.md @@ -0,0 +1,64 @@ +# easynav_mpc_controller + +[![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) + + +## Description +A Model Predictive Controller (MPC) implementation for Easy Navigation. + +## Authors and Maintainers +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Juan S. Cely G. + +## Supported ROS 2 Distributions +| Distribution | Status | +|---|---| +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) +- **Plugin Name:** `easynav_mpc_controller/MPCController` +- **Type:** `easynav::MPCController` +- **Base Class:** `easynav::ControllerMethodBase` +- **Library:** `easynav_mpc_controller` +- **Description:** A Model Predictive Controller (MPC) implementation for Easy Navigation. + +## Parameters +All parameters are declared under the plugin namespace, i.e., `//easynav_mpc_controller/MPCController/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.horizon_steps` | `int` | `5` | Number of time steps in the prediction horizon. | +| `.dt` | `double` | `0.1` | Integration time step (seconds). | +| `.safety_radius` | `double` | `0.35` | Safety radius to check possible collisions. | +| `.max_linear_velocity` | `double` | `1.5` | Maximum linear velocity (m/s). | +| `.max_angular_velocity` | `double` | `1.5` | Maximum angular velocity (rad/s). | +| `.verbose` | `bool` | `false` | Show data on terminal about Optimization. | + + +## Interfaces (Topics and Services) + +### Subscriptions and Publications +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| +| Publisher | `/mpc/path` | `nav_msgs/msg/path` | MPC trajectories generate by Predictive Component. | QoS depth=10 | +| Publisher | `/mpc/detection` | `sensor_msgs::msg::PointCloud2` | Detections used by collision checker. | QoS depth=10 | + + +### Services +This package does not create service servers or clients. + + +## NavState Keys +| Key | Type | Access | Notes | +|---|---|---|---| +| `path` | `nav_msgs::msg::Path` | **Read** | Target path to track. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose/state. | +| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Read** | Last commanded velocity (if provided in state). | +| `points` | `PointPerceptions` | **Read** | Perception point cloud(s) used for costs. | + + +## TF Frames +This controller does not explicitly publish or require TF frames in code. + +## License +Apache-2.0 diff --git a/controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml b/controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml new file mode 100644 index 00000000..cebf350e --- /dev/null +++ b/controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + A MPC implementation for a Controller. + + + + diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp new file mode 100644 index 00000000..acef3630 --- /dev/null +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -0,0 +1,105 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// #pragma once +#ifndef EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ +#define EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ + +#include +#include +#include +#include + +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" + +#include +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "easynav_core/ControllerMethodBase.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_sensors/types/PointPerception.hpp" + +#include "easynav_mpc_controller/MPCOptimizer.hpp" + +namespace easynav +{ + +/// \brief A MPC Controller. +class MPCController : public ControllerMethodBase +{ +public: + MPCController(); + + /// \brief Destructor. + ~MPCController() override; + + /// \brief Initializes parameters and MPC controller. + /// \throws std::runtime_error if initialization fails. + void on_initialize() override; + + /// \brief Updates the controller using the given NavState. + /// \param nav_state Current navigation state, including odometry and planned path. + void update_rt(NavState & nav_state) override; + + /// \brief Publishes the selected path by MPC controller + /// \param data MPCParameters object as data pointer + /// \param best_path Vector of velocities generated by MPC + /// \param path Path obtained from navstate + void publish_mpc_path( + void * data, const std::vector & best_path, + nav_msgs::msg::Path path); + + /// \brief Check if there is a possible collision + /// \param data MPCParameters object as data pointer + /// \param u Vector of velocities generated by MPC + void collision_checker(void * data, std::vector & u); + +protected: + int horizon_steps_{5}; ///< Prediction horizon for MPC. + double dt_{0.1}; ///< Time step for MPC. + double safety_radius_{0.35}; ///< Safety radius to avoid obstacles + double max_lin_vel_{1.5}; ///< Maximum linear velocity for MPC. + double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. + bool verbose_{false}; ///< Value to debug on terminal + double last_v_{0.0}; ///< Last value for linear velocity before than collision + double last_w_{0.0}; ///< Last value for angular velocity before than collision + bool collision_state_{false}; ///< Collision state flag + double collision_factor_{0.618033}; ///< Collision avoidance for recalculate velocities + + // Fallback goal tolerances if GoalManager does not publish them + double fallback_goal_pos_tol_{0.05}; ///< Default positional tolerance (meters). + double fallback_goal_yaw_tol_{0.05}; ///< Default angular tolerance (radians). + + std::unique_ptr optimizer_; ///< MPC optimizer + + rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path. + rclcpp::Publisher::SharedPtr detection_pub_; ///< Publisher for MPC obstacles. + +private: + geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. + +}; + +} // namespace easynav + +#endif // EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp new file mode 100644 index 00000000..40ca6ce6 --- /dev/null +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -0,0 +1,130 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// #pragma once +#ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ +#define EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ + +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/path.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" + +namespace easynav +{ + +/// \brief A MPC parameters class. +class MPCParameters +{ +public: + MPCParameters( + Eigen::Vector2d goal, + Eigen::Vector3d x0, + Eigen::Vector3d theta0, + const pcl::PointCloud & points, + int N, + double dt); + + /// \brief Destructor. + ~MPCParameters(); + + Eigen::Vector2d goal; ///< Goal pose (x,y) to optimizer. + Eigen::Vector3d x0; ///< Init pos (x,y,z). + Eigen::Vector3d theta0; ///< Init orientation (roll,pitch,yaw). + const pcl::PointCloud & points; ///< Filtered Point Cloud to detect collisions. + + /// \brief Get the number of horizont steps + /// \return integer value of amount of horizont step used in optimization + int get_steps(); + + /// \brief Get the differential time used by integrator in kinematic model + /// \return double value of time in seconds + double get_timestep(); + + /// \brief Get angular cost used in angle optimization + /// \return double value of angular cost + double get_angular_tracking_cost(); + + /// \brief Get effort cost matrix used in optimization + /// \return Eigen::Matrix2d as effort cost matrix + Eigen::Matrix2d get_effort_cost(); + + /// \brief Get tracking cost matrix used in optimization + /// \return Eigen::Matrix2d as tracking cost matrix + Eigen::Matrix2d get_tracking_cost(); + + /// \brief Get smooth cost matrix used in optimization + /// \return Eigen::Matrix2d as smooth cost matrix + Eigen::Matrix2d get_smooth_cost(); + +private: + int N_ {5}; ///< Horizont Step to optimize. + double dt_ {0.1}; ///< Differential time to integrate. + Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost Matrix + Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost Matrix + Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost Matrix + double qtheta_ {3.0}; ///< Angular cost value. + +}; + +/// \brief A MPC Optimizer class. +class MPCOptimizer +{ +public: + MPCOptimizer(); + + /// \brief Destructor. + ~MPCOptimizer(); + + /// \brief Kinematic model for a particle used by optmizer to stimate final position + /// \param x Eigen::Vector3d Initial position for robot (x,y,z) + /// \param q Eigen::Vector3d Initial angel for robot (roll, pitch , yaw) + /// \param v double lineal velocity + /// \param w double angular velocity + /// \param dt double differential time used for integration + /// \return Eigen::Vector3d Final state for robot (x,y,yaw) + Eigen::Vector3d kinematic_model( + const Eigen::Vector3d & x, + const Eigen::Vector3d & q, double v, double w, double dt); + + /// \brief Wrap for real cost function + /// \param u std::vector Velocity vector to be optimized + /// \param grad std::vector gradient values for optimizer. It is NOT used for this implementation. + /// \param data MPCParameter pointer with parameters used by optimizer + /// \return double cost value used by nlOpt in internal callback + double cost_function(const std::vector & u, [[maybe_unused]] + std::vector & grad, void * data); + + /// \brief Real cost function with static propierties + /// \param u std::vector Velocity vector to be optimized + /// \param grad std::vector gradient values for optimizer. It is NOT used for this implementation. + /// \param data MPCParameter pointer with parameters used by optimizer + /// \return double cost value used by nlOpt in internal callback + static double nlopt_cost_callback( + const std::vector & x, + std::vector & grad, void * data); + +}; + +/// \brief Struct used as element in callback. +struct NLoptCallbackData +{ + MPCOptimizer * optimizer; ///< Pointer to optimizer. + MPCParameters * params; ///< Pointer to parameter for optimizer. +}; + +} // namespace easynav + +#endif // EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml new file mode 100644 index 00000000..0a102524 --- /dev/null +++ b/controllers/easynav_mpc_controller/package.xml @@ -0,0 +1,38 @@ + + + + easynav_mpc_controller + 0.4.0 + Easy Navigation: MPC Controller package. + Juan S. Cely G. + Apache-2.0 + + ament_cmake + libnlopt-cxx-dev + libnlopt-dev + libnlopt0 + + easynav_common + easynav_core + easynav_system + easynav_sensors + pluginlib + tf2_ros + tf2 + geometry_msgs + nav_msgs + sensor_msgs + pcl_conversions + eigen + + rclcpp_lifecycle + easynav_simple_common + std_srvs + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp new file mode 100644 index 00000000..114382e5 --- /dev/null +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -0,0 +1,361 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Implementation of the MPCController class. + +#include "easynav_mpc_controller/MPCController.hpp" +#include "easynav_system/GoalManager.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +namespace easynav +{ + +MPCController::MPCController() {} + +MPCController::~MPCController() = default; + +void +MPCController::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->declare_parameter(plugin_name + ".dt", dt_); + node->declare_parameter(plugin_name + ".safety_radius", safety_radius_); + node->declare_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + node->declare_parameter(plugin_name + ".verbose", verbose_); + + node->declare_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); + node->declare_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + + node->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->get_parameter(plugin_name + ".dt", dt_); + node->get_parameter(plugin_name + ".safety_radius", safety_radius_); + node->get_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->get_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + node->get_parameter(plugin_name + ".verbose", verbose_); + + node->get_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); + node->get_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + + optimizer_ = std::make_unique(); + + mpc_path_pub_ = + node->create_publisher("/mpc/path", 10); + + detection_pub_ = + node->create_publisher("/mpc/detection", 10); +} + +void +MPCController::publish_mpc_path( + void * data, const std::vector & best_vel, + nav_msgs::msg::Path path) +{ + MPCParameters * params = reinterpret_cast(data); + nav_msgs::msg::Path mpc_path_; + + if (best_vel.size() > 0) { + mpc_path_.header.stamp = get_node()->now(); + mpc_path_.header.frame_id = path.header.frame_id; + for (size_t i = 0; i + 1 < best_vel.size(); i += 2) { + geometry_msgs::msg::PoseStamped pose_stamped; + double v = best_vel[i]; + double w = best_vel[i + 1]; + pose_stamped.header.frame_id = path.header.frame_id; + pose_stamped.header.stamp = path.header.stamp; + auto state = optimizer_->kinematic_model(params->x0, params->theta0, v, w, dt_); + pose_stamped.pose.position.x = state[0]; + pose_stamped.pose.position.y = state[1]; + mpc_path_.poses.push_back(pose_stamped); + } + mpc_path_pub_->publish(mpc_path_); + + } +} + +void +MPCController::collision_checker(void * data, std::vector & u) +{ + MPCParameters * params = reinterpret_cast(data); + double x_m = 0.0, y_m = 0.0, dist = 0.0, angle = 0.0; + size_t real_points = 0; + for (const auto & point : params->points) { + if (!std::isnan(point.x) || !std::isnan(point.y)) { + x_m += (point.x - params->x0[0]); + y_m += (point.y - params->x0[1]); + real_points++; + } + } + if (real_points != 0) { + x_m /= real_points; + y_m /= real_points; + dist = std::hypot(x_m, y_m); + angle = std::atan2(y_m, x_m) - params->theta0[2]; + if (dist < safety_radius_) { + if (!collision_state_) { + collision_state_ = true; + last_v_ = u[0]; + last_w_ = u[1]; + } + RCLCPP_WARN( + get_node()->get_logger(), + "[COLLISION] Collision detected at: [%f] m and Theta: [%f] degrees", dist, + (angle * 180.00 / M_PI) ); + last_v_ = collision_factor_ * last_v_ * safety_radius_ / dist; + last_w_ = collision_factor_ * last_w_ * safety_radius_ / dist; + u[0] = -last_v_; + u[1] = -last_w_; + } else { + collision_state_ = false; + } + } +} + +void +MPCController::update_rt(NavState & nav_state) +{ + // If navigation is IDLE, force zero velocity + if (nav_state.has("navigation_state")) { + const auto nav_state_val = nav_state.get("navigation_state"); + if (nav_state_val == easynav::GoalManager::State::IDLE) { + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; + nav_state.set("cmd_vel", cmd_vel_); + return; + } + } + + const auto & perceptions = nav_state.get_no_group(); + + if (!nav_state.has("path") || !nav_state.has("robot_pose") || perceptions.empty()) { + if (verbose_) { + std::cout << "No Path, No Points or No Robot Pose" << std::endl; + } + return; + } + + nav_msgs::msg::Path path = nav_state.get("path"); + if (path.poses.empty()) { + // If the path is empty, stop the robot + cmd_vel_.header.frame_id = path.header.frame_id; + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; + nav_state.set("cmd_vel", cmd_vel_); + return; + } + + // Build a local path that: + // 1) keeps only the segment that brings the robot closer to the goal, and + // 2) prepends a short straight segment from the robot pose to that segment. + const auto & robot_pose_msg = nav_state.get("robot_pose"); + const auto & robot_p = robot_pose_msg.pose.pose.position; + + // Goal is the last point of the planner path + const auto & goal_p = path.poses.back().pose.position; + + nav_msgs::msg::Path local_path; + local_path.header = path.header; + local_path.poses.clear(); + + // 1) Find the first index that actually brings us closer to the goal than our current pose + const double d_robot_goal = std::hypot(goal_p.x - robot_p.x, goal_p.y - robot_p.y); + std::size_t start_idx = 0; + for (std::size_t i = 0; i < path.poses.size(); ++i) { + const auto & pi = path.poses[i].pose.position; + const double d_pi_goal = std::hypot(goal_p.x - pi.x, goal_p.y - pi.y); + if (d_pi_goal <= d_robot_goal) { + start_idx = i; + break; + } + } + + // 2) Prepend a point at the robot position to ensure continuity from the current pose + geometry_msgs::msg::PoseStamped robot_ps; + robot_ps.header = path.header; + robot_ps.pose.position = robot_p; + robot_ps.pose.orientation = path.poses[start_idx].pose.orientation; + local_path.poses.push_back(robot_ps); + + // 3) Copy the remaining points from start_idx to the goal + for (std::size_t i = start_idx; i < path.poses.size(); ++i) { + local_path.poses.push_back(path.poses[i]); + } + + // Use the local path from now on + path = local_path; + + int num_elements = path.poses.size(); + size_t local_horizon; + if (num_elements > horizon_steps_) { + local_horizon = horizon_steps_; + } else { + local_horizon = num_elements - 1; + } + const auto & last_pose = path.poses[local_horizon].pose.position; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + const auto & filtered = PointPerceptionsOpsView(perceptions) + .filter({-2.0, -0.35, -1.0}, {0.0, 0.35, 1.0}) + .fuse(tf_info.map_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .collapse({NAN, NAN, 0.1}) + .downsample(0.1) + .as_points(); + + sensor_msgs::msg::PointCloud2 cloud_out; + pcl::toROSMsg(filtered, cloud_out); + cloud_out.header.frame_id = path.header.frame_id; + cloud_out.header.stamp = get_node()->now(); + detection_pub_->publish(cloud_out); + + const auto pose = nav_state.get("robot_pose").pose.pose; + double roll_, pitch_, yaw_; + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + tf2::Matrix3x3 m(q); + m.getRPY(roll_, pitch_, yaw_); + + // MPC Code + double minf; + std::vector u(2 * horizon_steps_, 0.0); + + auto params = MPCParameters( + Eigen::Vector2d(static_cast(last_pose.x), static_cast(last_pose.y)), + {pose.position.x, pose.position.y, pose.position.z}, + {roll_, pitch_, yaw_}, + filtered, + static_cast(horizon_steps_), + dt_); + + NLoptCallbackData cbdata{optimizer_.get(), ¶ms}; + + nlopt::opt opt(nlopt::LN_COBYLA, static_cast(u.size())); + opt.set_min_objective(easynav::MPCOptimizer::nlopt_cost_callback, &cbdata); + + std::vector lb(2 * horizon_steps_); + std::vector ub(2 * horizon_steps_); + for (int k = 0; k < horizon_steps_; k++) { + lb[2 * k] = -max_lin_vel_; + lb[2 * k + 1] = -max_ang_vel_; + ub[2 * k] = max_lin_vel_; + ub[2 * k + 1] = max_ang_vel_; + } + opt.set_lower_bounds(lb); + opt.set_upper_bounds(ub); + opt.set_xtol_rel(1e-3); + opt.set_ftol_rel(1e-3); + // opt.set_maxeval(1000); + + try { + nlopt::result result = opt.optimize(u, minf); + if (verbose_) { + if (result > 0) { + std::cerr << "Optimization Successful " << std::endl; + std::cout << "Result: " << result << std::endl; + } else { + std::cerr << "Optimization Unsuccessful " << std::endl; + } + } + + } catch (std::exception & e) { + std::cerr << "Optimization Error: " << e.what() << std::endl; + } + + if (ControllerMethodBase::collision_checker_active_) { + collision_checker(¶ms, u); + } + + // Final alignment phase with hysteresis on distance: + // - Enter when dist_to_goal <= 0.5 * pos_tol + // - Stay in this phase (even if dist grows slightly) until dist_to_goal > pos_tol + { + const auto & goal_pose = path.poses.back().pose; + + double pos_tol = fallback_goal_pos_tol_; + double yaw_tol = fallback_goal_yaw_tol_; + + if (nav_state.has("goal_tolerance.position")) { + pos_tol = nav_state.get("goal_tolerance.position"); + } + if (nav_state.has("goal_tolerance.yaw")) { + yaw_tol = nav_state.get("goal_tolerance.yaw"); + } + + const double dx_g = goal_pose.position.x - pose.position.x; + const double dy_g = goal_pose.position.y - pose.position.y; + const double dist_to_goal = std::hypot(dx_g, dy_g); + + const double yaw_goal = std::atan2( + 2.0 * (goal_pose.orientation.w * goal_pose.orientation.z + + goal_pose.orientation.x * goal_pose.orientation.y), + 1.0 - 2.0 * (goal_pose.orientation.y * goal_pose.orientation.y + + goal_pose.orientation.z * goal_pose.orientation.z)); + double e_theta_goal = std::atan2(std::sin(yaw_ - yaw_goal), std::cos(yaw_ - yaw_goal)); + + const double enter_dist = 0.5 * pos_tol; + + // Hysteresis based on distance to goal: start aligning when we are + // well inside the goal radius (enter_dist) and keep aligning until + // we move clearly outside (dist_to_goal > pos_tol). + const bool inside_hysteresis_band = (dist_to_goal <= pos_tol); + const bool should_enter_alignment = (dist_to_goal <= enter_dist); + + if ((inside_hysteresis_band && std::fabs(e_theta_goal) > yaw_tol) || + should_enter_alignment) + { + // Stay in place and rotate towards the goal orientation using a simple P controller + const double k_align = 1.0; + double vlin = 0.0; + double vrot = -k_align * e_theta_goal; + + vrot = std::clamp(vrot, -max_ang_vel_, max_ang_vel_); + + cmd_vel_.header.frame_id = path.header.frame_id; + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = vlin; + cmd_vel_.twist.angular.z = vrot; + + nav_state.set("cmd_vel", cmd_vel_); + publish_mpc_path(¶ms, u, path); + return; + } + } + + // Publish the computed velocity command + cmd_vel_.header.frame_id = path.header.frame_id; + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = u[0]; + cmd_vel_.twist.angular.z = u[1]; + + nav_state.set("cmd_vel", cmd_vel_); + + // Publish the path + publish_mpc_path(¶ms, u, path); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::MPCController, easynav::ControllerMethodBase) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp new file mode 100644 index 00000000..067b1495 --- /dev/null +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -0,0 +1,150 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Implementation of MPCParameters and MPCOptimizer classes. + +#include "easynav_mpc_controller/MPCOptimizer.hpp" + +namespace easynav +{ + +MPCParameters::MPCParameters( + Eigen::Vector2d goal, + Eigen::Vector3d x0, + Eigen::Vector3d theta0, + const pcl::PointCloud & points, + int N, + double dt) +: goal(goal), x0(x0), theta0(theta0), points(points), N_(N), dt_(dt) {} + +MPCParameters::~MPCParameters() = default; + +int +MPCParameters::get_steps() +{ + return N_; +} + +double +MPCParameters::get_timestep() +{ + return dt_; +} + +double +MPCParameters::get_angular_tracking_cost() +{ + return qtheta_; +} + +Eigen::Matrix2d +MPCParameters::get_effort_cost() +{ + return R_; +} + +Eigen::Matrix2d +MPCParameters::get_tracking_cost() +{ + return Q_; +} + +Eigen::Matrix2d +MPCParameters::get_smooth_cost() +{ + return Rd_; +} + +MPCOptimizer::MPCOptimizer() {} + +MPCOptimizer::~MPCOptimizer() = default; + +Eigen::Vector3d +MPCOptimizer::kinematic_model( + const Eigen::Vector3d & x, const Eigen::Vector3d & q, double v, + double w, double dt) +{ + Eigen::Vector3d x_k1; + x_k1[0] = x[0] + v * cos(q[2]) * dt; + x_k1[1] = x[1] + v * sin(q[2]) * dt; + x_k1[2] = q[2] + w * dt; + return x_k1; +} + +double +MPCOptimizer::cost_function( + const std::vector & u, + [[maybe_unused]] std::vector & grad, void * data) +{ + MPCParameters * params = reinterpret_cast(data); + + Eigen::Vector3d position = params->x0; + Eigen::Vector3d orientation = params->theta0; + Eigen::Vector3d state; + int N = params->get_steps(); + double dt = params->get_timestep(); + double qtheta = params->get_angular_tracking_cost(); + double cost = 0.0; + + Eigen::Matrix2d R = params->get_effort_cost(); + Eigen::Matrix2d Q = params->get_tracking_cost(); + Eigen::Matrix2d Rd = params->get_smooth_cost(); + + for (int i = 0; i < N; ++i) { + double v = u[2 * i]; + double w = u[2 * i + 1]; + double dv, dw; + if (i < (N - 1)) { + dv = u[2 * (i + 1)] - u[2 * i]; + dw = u[2 * (i + 1) + 1] - u[2 * i + 1]; + } else { + dv = 0.0; + dw = 0.0; + } + + state = MPCOptimizer::kinematic_model(position, orientation, v, w, dt); + + Eigen::Vector2d pos = state.head<2>(); + Eigen::Vector2d error = params->goal - pos; + double error_theta = (atan2((error[1]), (error[0]))) - state[2]; + Eigen::Vector2d uk(v, w); + Eigen::Vector2d duk(dv, dw); + + // Tracking cost + cost += Q(0, 0) * error[0] * error[0] + Q( + 1, + 1) * error[1] * error[1] + qtheta * error_theta * error_theta; + // Effort Cost + cost += R(0, 0) * v * v + R(1, 1) * w * w; + // Smooth Cost + cost += Rd(0, 0) * dv * dv + Rd(1, 1) * dw * dw; + } + + return cost; + +} + +double +MPCOptimizer::nlopt_cost_callback( + const std::vector & x, + std::vector & grad, + void * data) +{ + auto * cbdata = static_cast(data); + return cbdata->optimizer->cost_function(x, grad, cbdata->params); +} + +} // namespace easynav diff --git a/controllers/easynav_mppi_controller/CHANGELOG.rst b/controllers/easynav_mppi_controller/CHANGELOG.rst index 7c4dda88..dd3b7b6d 100644 --- a/controllers/easynav_mppi_controller/CHANGELOG.rst +++ b/controllers/easynav_mppi_controller/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package easynav_mppi_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Remove C++20/C++23 features and update to new MethodBase interface +* Merge branch 'set_robot_frame' into frames-fix-pr-40 +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Include package in cmake and package.xml +* Fix link error when running MPPI in Realease +* Stop controllers at IDLE +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel, estherag + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index d702c4f5..19b6bcdd 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -5,16 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_system REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io) @@ -32,7 +32,11 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + easynav_system::easynav_system + easynav_sensors::easynav_sensors tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -73,8 +77,11 @@ pluginlib_export_plugin_description_file(easynav_core easynav_mppi_controller_pl ament_export_dependencies( easynav_common easynav_core + easynav_sensors pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs visualization_msgs diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 655e8568..238d8b44 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -2,22 +2,27 @@ [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) - ## Description + A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Jose Miguel Guerrero Hernandez ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_mppi_controller/MPPIController` - **Type:** `easynav::MPPIController` - **Base Class:** `easynav::ControllerMethodBase` @@ -25,8 +30,12 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig - **Description:** A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_mppi_controller/MPPIController/...`. +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. + | Name | Type | Default | Description | |---|---|---:|---| | `.num_samples` | `int` | `100` | Number of trajectory rollouts per iteration. | @@ -40,21 +49,21 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.fov` | `double` | `M_PI/2.0` | Field of view used in trajectory sampling (radians). | | `.safety_radius` | `double` | `0.6` | Safety radius around the robot (meters). | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Publisher | `/mppi/candidates` | `visualization_msgs/msg/MarkerArray` | MPPI candidate trajectories as markers. | QoS depth=10 | | Publisher | `/mppi/optimal_path` | `visualization_msgs/msg/MarkerArray` | Optimal MPPI trajectory as markers. | QoS depth=10 | - ### Services -This package does not create service servers or clients. +This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `path` | `nav_msgs::msg::Path` | **Read** | Target path to track. | @@ -62,9 +71,10 @@ This package does not create service servers or clients. | `points` | `PointPerceptions` | **Read** | Perception point cloud(s) used for costs. | | `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Read** | Last commanded velocity (if provided in state). | - ## TF Frames + This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only + +Apache-2.0 diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp index ac53bfe2..0f3f03c9 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -1,29 +1,32 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #include -#include #include -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" #include "easynav_mppi_controller/MPPIOptimizer.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" namespace easynav { @@ -38,8 +41,8 @@ class MPPIController : public ControllerMethodBase ~MPPIController() override; /// \brief Initializes parameters and MPPI controller. - /// \return std::expected success or error message. - std::expected on_initialize() override; + /// \throws std::runtime_error if initialization fails. + void on_initialize() override; /// \brief Updates the controller using the given NavState. /// \param nav_state Current navigation state, including odometry and planned path. diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp index ba9ba858..d900e166 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp @@ -1,8 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. // #pragma once #ifndef EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 7e9ee33a..de1542b9 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -2,17 +2,21 @@ easynav_mppi_controller - 0.0.2 + 0.4.0 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake easynav_common easynav_core + easynav_system + easynav_sensors pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs visualization_msgs diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index 42d45c38..9947db91 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -1,31 +1,26 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the MPPIController class. -#include -#include "tf2/utils.hpp" - #include "easynav_mppi_controller/MPPIController.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_system/GoalManager.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -36,7 +31,7 @@ MPPIController::MPPIController() {} MPPIController::~MPPIController() = default; -std::expected +void MPPIController::on_initialize() { auto node = get_node(); @@ -64,21 +59,21 @@ MPPIController::on_initialize() node->get_parameter(plugin_name + ".fov", fov_); node->get_parameter(plugin_name + ".safety_radius", safety_radius_); - optimizer_ = std::make_unique(num_samples_, horizon_steps_, dt_, lambda_, + optimizer_ = std::make_unique( + num_samples_, horizon_steps_, dt_, lambda_, max_lin_vel_, max_ang_vel_, fov_, safety_radius_); mppi_candidates_pub_ = node->create_publisher("/mppi/candidates", 10); mppi_optimal_pub_ = node->create_publisher("/mppi/optimal_path", 10); - - return {}; } void MPPIController::publish_mppi_markers( const std::vector>> & all_trajs, const std::vector> & best_traj) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); visualization_msgs::msg::MarkerArray candidates; visualization_msgs::msg::MarkerArray optimal; int id = 0; @@ -86,7 +81,7 @@ void MPPIController::publish_mppi_markers( // Candidates in blue for (const auto & traj : all_trajs) { visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; + marker.header.frame_id = tf_info.map_frame; marker.header.stamp = rclcpp::Clock().now(); marker.ns = "mppi_candidates"; marker.id = id++; @@ -111,7 +106,7 @@ void MPPIController::publish_mppi_markers( // Best trajectory in red visualization_msgs::msg::Marker best_marker; - best_marker.header.frame_id = "map"; + best_marker.header.frame_id = tf_info.map_frame; best_marker.header.stamp = rclcpp::Clock().now(); best_marker.ns = "mppi_optimal_path"; best_marker.id = id++; @@ -142,11 +137,32 @@ void MPPIController::publish_mppi_markers( void MPPIController::update_rt(NavState & nav_state) { + // If navigation is IDLE, force zero velocity + if (nav_state.has("navigation_state")) { + const auto nav_state_val = nav_state.get("navigation_state"); + if (nav_state_val == easynav::GoalManager::State::IDLE) { + twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.twist.linear.x = 0.0; + twist_stamped_.twist.angular.z = 0.0; + nav_state.set("cmd_vel", twist_stamped_); + + // Also clear visualization markers when idle + visualization_msgs::msg::MarkerArray clear_markers; + visualization_msgs::msg::Marker delete_all; + delete_all.action = visualization_msgs::msg::Marker::DELETEALL; + clear_markers.markers.push_back(delete_all); + + mppi_candidates_pub_->publish(clear_markers); + mppi_optimal_pub_->publish(clear_markers); + return; + } + } + if (!nav_state.has("path") || !nav_state.has("robot_pose")) { return; } - const auto path = nav_state.get("path"); + const auto & path = nav_state.get("path"); if (path.poses.empty()) { // If the path is empty, stop the robot and clear markers @@ -166,20 +182,21 @@ MPPIController::update_rt(NavState & nav_state) return; } - const auto pose = nav_state.get("robot_pose").pose.pose; - const auto perceptions = nav_state.get("points"); - + const auto & pose = nav_state.get("robot_pose").pose.pose; + const auto & perceptions = nav_state.get_no_group(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) - .fuse("map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .fuse(tf_info.map_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) - ->downsample(0.1) + .downsample(0.1) .as_points(); if (filtered.empty()) { - RCLCPP_WARN(get_node()->get_logger(), - "No valid points available for MPPI optimization, using the path only."); + RCLCPP_WARN( + get_node()->get_logger(), + "No valid points available for MPPI optimization, using the path only."); } // Compute the control using MPPI with points diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp index 8205169f..2b03cca1 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp @@ -1,13 +1,10 @@ #include "easynav_mppi_controller/MPPIOptimizer.hpp" #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include #include -#include - -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" namespace easynav { @@ -46,8 +43,9 @@ MPPIOptimizer::simulate_trajectory( if (has_path) { // Calculate the index in the path based on the current step - size_t path_idx = std::min(static_cast((i * path.poses.size()) / horizon_steps), - path.poses.size() - 1); + size_t path_idx = std::min( + static_cast((i * path.poses.size()) / horizon_steps), + path.poses.size() - 1); double target_x = path.poses[path_idx].pose.position.x; double target_y = path.poses[path_idx].pose.position.y; @@ -103,12 +101,14 @@ double MPPIOptimizer::compute_cost( double dist = std::hypot(dx, dy); // Heading error is calculated based on the initial yaw - double heading_penalty = heading_error(initial_yaw, path.poses[idx].pose.position.x, - path.poses[idx].pose.position.y, x, y); + double heading_penalty = heading_error( + initial_yaw, path.poses[idx].pose.position.x, + path.poses[idx].pose.position.y, x, y); // FOV penalty: discourage trajectories outside robot's view - double angle_to_goal = heading_error(initial_yaw, trajectory.back().first, - trajectory.back().second, x, y); + double angle_to_goal = heading_error( + initial_yaw, trajectory.back().first, + trajectory.back().second, x, y); double fov_penalty = std::pow(std::max(0.0, angle_to_goal - fov_), 2); // Accumulate penalties @@ -245,8 +245,9 @@ MPPIResult MPPIOptimizer::compute_control( } // Softmin: Find minimum cost among samples - auto best_sample_it = std::min_element(samples.begin(), samples.end(), - [](const auto & a, const auto & b) {return a.cost < b.cost;}); + auto best_sample_it = std::min_element( + samples.begin(), samples.end(), + [](const auto & a, const auto & b) {return a.cost < b.cost;}); // Best trajectory and cost best_traj = all_trajs[std::distance(samples.begin(), best_sample_it)]; @@ -269,7 +270,7 @@ MPPIResult MPPIOptimizer::compute_control( denom += sample.cost; } - // Weighted average of velocities + // Weighted average of velocities double vlin = 0.0, vrot = 0.0; for (const auto & sample : samples) { vlin += sample.v * sample.cost / denom; diff --git a/controllers/easynav_regulated_pp_controller/CHANGELOG.rst b/controllers/easynav_regulated_pp_controller/CHANGELOG.rst new file mode 100644 index 00000000..1b79b819 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/CHANGELOG.rst @@ -0,0 +1,20 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_regulated_pp_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Fix overshoot when DWPP is activated +* Fix license and copyright +* Port of Regulated Pure Pursuit. Initial commit +* Contributors: Francisco Martín Rico + +0.3.1 (2026-02-27 17:53) +------------------------ + +0.2.1 (2026-02-27 14:48) +------------------------ + +0.0.2 (2025-10-12) +------------------ diff --git a/controllers/easynav_regulated_pp_controller/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/CMakeLists.txt new file mode 100644 index 00000000..8f54c652 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_regulated_pp_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_system REQUIRED) +find_package(easynav_sensors REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) + + +add_library(${PROJECT_NAME} SHARED + src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + easynav_system::easynav_system + easynav_sensors::easynav_sensors + tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + pluginlib::pluginlib + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the controller plugin +pluginlib_export_plugin_description_file(easynav_core easynav_regulated_pp_controller_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + easynav_system + easynav_sensors + pluginlib + tf2_ros + tf2 + tf2_geometry_msgs + geometry_msgs + nav_msgs + std_msgs +) +ament_package() diff --git a/controllers/easynav_regulated_pp_controller/README.md b/controllers/easynav_regulated_pp_controller/README.md new file mode 100644 index 00000000..03be5d46 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/README.md @@ -0,0 +1,170 @@ +# easynav_regulated_pp_controller + +Port of the [Nav2 Regulated Pure Pursuit Controller](https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html) +to the EasyNav plugin architecture (`easynav::ControllerMethodBase`), including its optional +Dynamic Window Pure Pursuit (DWPP) extension. + +- S. Macenski, S. Singh, F. Martin, J. Gines, [**Regulated Pure Pursuit for Robot Path Tracking**](https://arxiv.org/abs/2305.20026). Autonomous Robots, 2023. +- Fumiya Ohnishi and Masaki Takahashi, **DWPP: Dynamic Window Pure Pursuit Considering Velocity and Acceleration Constraints**, arXiv:2601.15006, 2026. + +```bibtex +@article{macenski2023regulated, + title={Regulated Pure Pursuit for Robot Path Tracking}, + author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines}, + year={2023}, + journal = {Autonomous Robots} +} +``` + +## Algorithm + +The controller finds a lookahead ("carrot") point on the path at a given distance from the +robot and drives towards it, following the classic Pure Pursuit geometry: the curvature needed +to reach the carrot point is `k = 2y / (x^2+y^2)` in the robot's local frame. On top of that, it +adds the *regulation* terms from the paper above: + +- **Velocity-scaled lookahead distance** (Adaptive Pure Pursuit): the lookahead distance grows + with speed, clamped between `min_lookahead_dist` and `max_lookahead_dist`. +- **Curvature regulation**: linear velocity is reduced on sharp turns (small turning radius). +- **Obstacle-proximity regulation**: linear velocity is reduced close to obstacles. +- **Approach-to-goal regulation**: linear velocity is reduced smoothly as the robot nears the + end of the path. +- **Rotate to rough heading / rotate to goal heading**: the robot rotates in place first if the + carrot (or the final goal) is at a large angle from the current heading. +- **Dynamic Window Pure Pursuit (optional)**: instead of directly applying `angular_vel = linear_vel * curvature`, + it searches the reachable (velocity, angular velocity) space under the configured + acceleration/deceleration limits for the point that best matches the desired curvature. + +## Adaptations from the Nav2 version + +EasyNav's design differs from Nav2's `controller_server` in ways that require some adaptations: + +- **No costmap.** EasyNav controllers only have access to the robot pose, the path, and fused + point-cloud perceptions (`PointPerception`) via the `NavState` blackboard — there is no + costmap or inflation layer to query. The cost-based linear velocity regulation term + (`use_cost_regulated_linear_velocity_scaling` in Nav2) is therefore replaced by an equivalent + term, `use_obstacle_regulated_linear_velocity_scaling`, driven by the distance to the nearest + point in the fused perception cloud within the lookahead corridor, rather than by inflated + costmap cost (see `heuristics::obstacleConstraint` in + [`regulation_functions.hpp`](include/easynav_regulated_pp_controller/regulation_functions.hpp)). +- **No per-controller collision-arc checking.** Nav2's RPP throws a `NoValidControl` exception + when it predicts a collision along a forward-simulated arc. EasyNav already performs this kind + of safety stop uniformly for every controller in `ControllerMethodBase` (see the shared + `colision_checker.*` parameters), so this controller does not duplicate it. +- **No separate goal-checker plugin.** Nav2 relies on an independent `GoalChecker` plugin to + decide when the robot has reached the goal. This controller checks the distance/angle to the + last path pose directly, using `goal_tolerance.position` / `goal_tolerance.yaw` from the + `NavState` if `GoalManager` has published them, or the `xy_goal_tolerance` / + `yaw_goal_tolerance` parameters otherwise — consistent with how other EasyNav controllers + (e.g. `easynav_simple_controller`, `easynav_mpc_controller`) handle it. +- **No TF-based path transform.** EasyNav controllers assume `path` and `robot_pose` are already + expressed in the same frame (see `easynav_simple_controller`, `easynav_mpc_controller`), so + there is no `transformPathInTargetFrame` step. The path-to-robot-frame conversion needed by the + Pure Pursuit geometry (curvature is only meaningful with the robot at the local origin facing + +x) is done analytically from the robot pose instead of via `tf2`. +- **No plan pruning upstream.** Nav2's `controller_server` prunes the global plan to a local + window starting at (or near) the robot before handing it to the controller. EasyNav's `path` in + `NavState` is the full, un-pruned plan, so the carrot search (`getLookAheadPoint`) explicitly + starts from the path pose closest to the robot (`findClosestPoseIndex`) rather than from index 0 + of the array. Skipping this step is a real trap, not just a style choice: once the robot has + travelled more than a lookahead distance away from the path's start pose, that start pose itself + would satisfy "farther than the lookahead distance", so a naive search from index 0 keeps + returning it as the carrot for the rest of the path — freezing the carrot behind the robot, + producing bogus large curvature/angle values, and making the controller rotate-in-place far more + than it should. +- **No `setSpeedLimit` API.** EasyNav has no equivalent to Nav2's dynamic speed-limiting + interface, so it is not ported. + +## Parameters + +| Parameter | Description | +|-----|----| +| `max_linear_vel` | Maximum linear velocity. | +| `min_linear_vel` | Minimum linear velocity, used when `use_dynamic_window` is `true`. | +| `max_angular_vel` / `min_angular_vel` | Angular velocity bounds, used when `use_dynamic_window` is `true`. | +| `max_linear_accel` / `max_linear_decel` | Linear acceleration/deceleration bounds, used when `use_dynamic_window` is `true`. | +| `max_angular_accel` / `max_angular_decel` | Angular acceleration/deceleration bounds; also used by `rotate_to_heading`. | +| `lookahead_dist` | Fixed lookahead distance to find the carrot point. | +| `min_lookahead_dist` / `max_lookahead_dist` | Bounds for the velocity-scaled lookahead distance. | +| `lookahead_time` | Gain used to scale the lookahead distance by the current speed. | +| `use_velocity_scaled_lookahead_dist` | Use velocity-scaled lookahead distance instead of the fixed `lookahead_dist`. | +| `rotate_to_heading_angular_vel` | Angular velocity used while rotating in place. | +| `use_rotate_to_heading` | Enable rotate-in-place behaviors (rough path heading and final goal heading). | +| `rotate_to_heading_min_angle` | Angle to the carrot beyond which the robot rotates in place first. | +| `use_regulated_linear_velocity_scaling` | Enable curvature-based velocity regulation. | +| `regulated_linear_scaling_min_radius` | Turning radius below which curvature regulation kicks in. | +| `regulated_linear_scaling_min_speed` | Minimum velocity kept under regulation. | +| `use_fixed_curvature_lookahead` | Use a separate, fixed lookahead distance to compute the regulation curvature. | +| `curvature_lookahead_dist` | Distance of the fixed curvature lookahead, if enabled. | +| `interpolate_curvature_after_goal` | Extrapolate the curvature carrot past the goal to avoid end-of-path oscillation. | +| `use_obstacle_regulated_linear_velocity_scaling` | Enable obstacle-proximity velocity regulation (EasyNav adaptation of Nav2's cost-based term). | +| `obstacle_scaling_dist` | Distance below which obstacle regulation is triggered. | +| `obstacle_scaling_gain` | Gain (`<= 1.0`) applied when scaling down the velocity near obstacles. | +| `min_approach_linear_velocity` | Minimum velocity while approaching the goal. | +| `approach_velocity_scaling_dist` | Remaining path distance at which approach-to-goal slow-down starts. | +| `allow_reversing` | Allow driving backwards when the carrot point is behind the robot. | +| `use_dynamic_window` | Enable the Dynamic Window Pure Pursuit (DWPP) extension. | +| `xy_goal_tolerance` / `yaw_goal_tolerance` | Fallback goal tolerances used if `GoalManager` has not published `goal_tolerance.*` in the `NavState`. | + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current carrot point on the path. | +| `curvature_lookahead_point` | `geometry_msgs/PointStamped` | The fixed curvature lookahead point, when `use_fixed_curvature_lookahead` is enabled. | +| `is_rotating_to_heading` | `std_msgs/Bool` | Whether the controller is currently rotating in place. | + +## Example configuration + +```yaml +controller: + ros__parameters: + controller_plugin: "easynav_regulated_pp_controller/RegulatedPurePursuitController" + RegulatedPurePursuitController: + max_linear_vel: 0.5 + min_linear_vel: -0.5 + max_angular_vel: 2.5 + min_angular_vel: -2.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + max_angular_accel: 3.2 + max_angular_decel: 3.2 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + use_velocity_scaled_lookahead_dist: false + rotate_to_heading_angular_vel: 1.8 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + use_obstacle_regulated_linear_velocity_scaling: false + obstacle_scaling_dist: 0.3 + obstacle_scaling_gain: 1.0 + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 1.0 + allow_reversing: false + use_dynamic_window: false + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 +``` + +## Tuning notes + +Same guidance as upstream Nav2 applies: + +- For Adaptive Pure Pursuit behavior, disable all boolean regulation parameters except + `use_velocity_scaled_lookahead_dist`, and tune `lookahead_time`, `min_lookahead_dist` and + `max_lookahead_dist`. +- For plain Pure Pursuit behavior, disable all boolean parameters and set + `approach_velocity_scaling_dist: 0.0` to disable approach-to-goal slow-down, then tune + `lookahead_dist`. +- `use_obstacle_regulated_linear_velocity_scaling` can over-trigger in tightly cluttered + environments (every point in a narrow corridor is "close"); tune `obstacle_scaling_dist` and + `obstacle_scaling_gain` for the platform's environment, the same way `cost_scaling_dist` / + `cost_scaling_gain` are tuned in Nav2. diff --git a/controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml b/controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml new file mode 100644 index 00000000..c5c47a8c --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + EasyNav port of the Nav2 Regulated Pure Pursuit Controller, with optional Dynamic Window Pure Pursuit (DWPP) extension. + + + + diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp new file mode 100644 index 00000000..607e2075 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -0,0 +1,201 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// and is a derivative work of nav2_regulated_pure_pursuit_controller, +// ported to the EasyNav plugin architecture. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_REGULATED_PP_CONTROLLER__REGULATEDPUREPURSUITCONTROLLER_HPP_ +#define EASYNAV_REGULATED_PP_CONTROLLER__REGULATEDPUREPURSUITCONTROLLER_HPP_ + +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/header.hpp" + +#include "easynav_core/ControllerMethodBase.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_sensors/types/PointPerception.hpp" + +namespace easynav +{ + +/// \brief Port of nav2_regulated_pure_pursuit_controller to EasyNav. +/// +/// Implements the Regulated Pure Pursuit algorithm described in: +/// S. Macenski, S. Singh, F. Martin, J. Gines, "Regulated Pure Pursuit for Robot Path Tracking", +/// Autonomous Robots, 2023 (https://arxiv.org/abs/2305.20026), plus the optional Dynamic Window +/// Pure Pursuit (DWPP) extension. See the package README for the adaptations made to fit +/// EasyNav's design (no costmap, no separate goal-checker/TF-transform plugins). +class RegulatedPurePursuitController : public ControllerMethodBase +{ +public: + RegulatedPurePursuitController(); + + /// \brief Destructor. + ~RegulatedPurePursuitController() override; + + /// \brief Declares and reads parameters. + /// \throws std::runtime_error on initialization failure. + void on_initialize() override; + + /// \brief Computes the next velocity command from the current NavState. + /// \param nav_state Current navigation state, including odometry and planned path. + void update_rt(NavState & nav_state) override; + +protected: + // --- Lookahead --- + double lookahead_dist_{0.6}; ///< Fixed lookahead distance (m). + double min_lookahead_dist_{0.3}; ///< Minimum velocity-scaled lookahead distance (m). + double max_lookahead_dist_{0.9}; ///< Maximum velocity-scaled lookahead distance (m). + double lookahead_time_{1.5}; ///< Lookahead gain: seconds projected at current speed. + bool use_velocity_scaled_lookahead_dist_{false}; ///< Use velocity-scaled lookahead distance. + + // --- Velocity / acceleration limits --- + double max_linear_vel_{0.5}; ///< Maximum linear velocity (m/s). + double min_linear_vel_{-0.5}; ///< Minimum linear velocity, used when use_dynamic_window is true (m/s). + double max_angular_vel_{2.5}; ///< Maximum angular velocity (rad/s). + double min_angular_vel_{-2.5}; ///< Minimum angular velocity, used when use_dynamic_window is true (rad/s). + double max_linear_accel_{2.5}; ///< Maximum linear acceleration (m/s^2). + double max_linear_decel_{2.5}; ///< Maximum linear deceleration (m/s^2). + double max_angular_accel_{3.2}; ///< Maximum angular acceleration (rad/s^2). + double max_angular_decel_{3.2}; ///< Maximum angular deceleration (rad/s^2). + bool use_dynamic_window_{false}; ///< Use the Dynamic Window Pure Pursuit (DWPP) extension. + bool allow_reversing_{false}; ///< Allow driving backwards when the carrot is behind the robot. + + // --- Rotate to heading --- + bool use_rotate_to_heading_{true}; ///< Enable rotate-in-place behaviors. + double rotate_to_heading_angular_vel_{1.8}; ///< Angular velocity used while rotating in place. + double rotate_to_heading_min_angle_{0.785}; ///< Angle to path beyond which to rotate in place. + + // --- Curvature regulation --- + bool use_regulated_linear_velocity_scaling_{true}; ///< Enable curvature-based regulation. + double regulated_linear_scaling_min_radius_{0.9}; ///< Turning radius that triggers regulation. + double regulated_linear_scaling_min_speed_{0.25}; ///< Minimum speed kept under regulation. + bool use_fixed_curvature_lookahead_{false}; ///< Use a separate lookahead for curvature. + double curvature_lookahead_dist_{1.0}; ///< Distance of the fixed curvature lookahead. + bool interpolate_curvature_after_goal_{false}; ///< Extrapolate curvature carrot past the goal. + + // --- Obstacle-proximity regulation (adapts the costmap-based term to EasyNav's perception) --- + bool use_obstacle_regulated_linear_velocity_scaling_{false}; ///< Enable obstacle-proximity regulation. + double obstacle_scaling_dist_{0.3}; ///< Distance below which obstacle regulation is triggered (m). + double obstacle_scaling_gain_{1.0}; ///< Gain (<=1.0) applied when scaling down the velocity. + + // --- Approach to goal --- + double min_approach_linear_velocity_{0.05}; ///< Minimum linear velocity while approaching goal. + double approach_velocity_scaling_dist_{1.0}; ///< Remaining-path distance at which to start slowing. + + // --- Goal tolerances (fallback values if "goal_tolerance.*" is not present in NavState) --- + double xy_goal_tolerance_{0.25}; ///< Fallback positional tolerance to the goal (m). + double yaw_goal_tolerance_{0.25}; ///< Fallback angular tolerance to the goal (rad). + + rclcpp::Publisher::SharedPtr lookahead_point_pub_; + rclcpp::Publisher::SharedPtr curvature_lookahead_point_pub_; + rclcpp::Publisher::SharedPtr is_rotating_to_heading_pub_; + + rclcpp::Time last_update_ts_; ///< Timestamp of the last control update. + double last_linear_vel_{0.0}; ///< Last commanded linear velocity (open-loop feedback). + double last_angular_vel_{0.0}; ///< Last commanded angular velocity (open-loop feedback). + bool is_rotating_to_heading_{false}; ///< Whether the controller is currently rotating in place. + geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. + + /// \brief Publishes a zero-velocity command and resets the open-loop velocity feedback. + void stop(NavState & nav_state, const std_msgs::msg::Header & header); + + /// \brief Computes the lookahead distance to search for the carrot pose. + /// \param linear_vel Linear velocity used to scale the lookahead distance, if enabled. + double getLookAheadDistance(double linear_vel) const; + + /// \brief Index of the path pose closest to \p robot_position. + /// + /// Since EasyNav controllers receive the full, un-pruned global path (see the README), the + /// search for the carrot pose must start from this index instead of from the beginning of the + /// path: otherwise, once the robot has advanced more than a lookahead distance away from the + /// path's start pose, that start pose itself would (incorrectly) be picked up as "farther than + /// the lookahead distance" and returned as the carrot, even though it lies behind the robot. + static std::size_t findClosestPoseIndex( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position); + + /// \brief Finds the pose on the path at (approximately) \p lookahead_dist from \p robot_position. + /// + /// Walks the path (in the same global frame as the robot pose), starting from the pose closest + /// to the robot (see \ref findClosestPoseIndex), looking for the first pose from there onwards + /// that is farther than \p lookahead_dist from the robot, then interpolates the exact carrot + /// position on the segment leading to it via a circle/segment intersection. If the remainder of + /// the path is closer than \p lookahead_dist, the path's last pose is returned (extrapolated one + /// extra segment if \p interpolate_after_end is true and the path has at least two poses). + static geometry_msgs::msg::Point getLookAheadPoint( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position, + double lookahead_dist, + bool interpolate_after_end = false); + + /// \brief Intersection of the segment [p1, p2] with the circle of radius \p r centered at \p center. + /// \return The intersection point closer to \p p2, or \p p2 itself if there is no real intersection. + static geometry_msgs::msg::Point circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r, + const geometry_msgs::msg::Point & center); + + /// \brief Expresses a global-frame point in the robot's local frame (robot at origin, facing +x). + static geometry_msgs::msg::Point toRobotFrame( + const geometry_msgs::msg::Point & point, + const geometry_msgs::msg::Pose & robot_pose, + double robot_yaw); + + /// \brief Whether the robot should rotate in place towards \p angle_to_path. + bool shouldRotateToPath(double angle_to_path) const; + + /// \brief Computes a kinematically-feasible rotate-in-place command towards \p angle_to_target. + void rotateToHeading( + double & linear_vel, double & angular_vel, double angle_to_target, double dt) const; + + /// \brief Applies curvature and obstacle-proximity regulation, then approach-to-goal scaling. + void applyConstraints( + double curvature, + double min_obstacle_distance, + double remaining_path_distance, + double euclidean_dist_to_goal, + double & linear_vel) const; + + /// \brief Distance (m) from the robot to the nearest fused perception point in front of it, + /// within \p max_range. Returns +infinity if there is no perception data or no nearby point. + double computeMinObstacleDistance(NavState & nav_state, double max_range) const; + + /// \brief Integrated path length from the robot position to the end of \p path. + static double remainingPathDistance( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position); + + /// \brief Publishes a PointStamped marker for a carrot pose, if the publisher has subscribers. + void publishCarrot( + const rclcpp::Publisher::SharedPtr & pub, + const geometry_msgs::msg::Point & carrot, + const std_msgs::msg::Header & header) const; +}; + +} // namespace easynav + +#endif // EASYNAV_REGULATED_PP_CONTROLLER__REGULATEDPUREPURSUITCONTROLLER_HPP_ diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp new file mode 100644 index 00000000..3579ac33 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp @@ -0,0 +1,328 @@ +// Copyright (c) 2025 Fumiya Ohnishi +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Dynamic Window Pure Pursuit (DWPP) helper functions, ported from +/// nav2_regulated_pure_pursuit_controller. These are plain math functions over velocities and +/// accelerations, so they carry over to EasyNav unchanged. +/// +/// Fumiya Ohnishi and Masaki Takahashi, "DWPP: Dynamic Window Pure Pursuit Considering Velocity +/// and Acceleration Constraints", arXiv:2601.15006, 2026. + +#ifndef EASYNAV_REGULATED_PP_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ +#define EASYNAV_REGULATED_PP_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" + +namespace easynav +{ + +namespace dynamic_window_pure_pursuit +{ + +struct DynamicWindowBounds +{ + double max_linear_vel; + double min_linear_vel; + double max_angular_vel; + double min_angular_vel; +}; + +/** + * @brief Compute the dynamic window (feasible velocity bounds) based on + * the current speed and the given velocity and acceleration constraints. + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum allowable linear velocity + * @param min_linear_vel Minimum allowable linear velocity + * @param max_angular_vel Maximum allowable angular velocity + * @param min_angular_vel Minimum allowable angular velocity + * @param max_linear_accel Maximum allowable linear acceleration + * @param max_linear_decel Maximum allowable linear deceleration + * @param max_angular_accel Maximum allowable angular acceleration + * @param max_angular_decel Maximum allowable angular deceleration + * @param dt Control duration + * @return Computed dynamic window's velocity bounds + */ +inline DynamicWindowBounds computeDynamicWindow( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt +) +{ + DynamicWindowBounds dynamic_window; + constexpr double Eps = 1e-3; + + // function to compute dynamic window for a single dimension + auto compute_window = [&](const double & current_vel, const double & max_vel, + const double & min_vel, const double & max_accel, const double & max_decel) + { + double candidate_max_vel = 0.0; + double candidate_min_vel = 0.0; + + if (current_vel > Eps) { + // if the current velocity is positive, acceleration means an increase in speed + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_decel * dt; + } else if (current_vel < -Eps) { + // if the current velocity is negative, acceleration means a decrease in speed + candidate_max_vel = current_vel + max_decel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } else { + // if the current velocity is zero, allow acceleration in both directions. + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } + + // clip to max/min velocity limits + double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); + double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel); + }; + + // linear velocity + std::tie(dynamic_window.max_linear_vel, dynamic_window.min_linear_vel) = + compute_window( + current_speed.linear.x, max_linear_vel, min_linear_vel, + max_linear_accel, max_linear_decel); + + // angular velocity + std::tie(dynamic_window.max_angular_vel, dynamic_window.min_angular_vel) = + compute_window( + current_speed.angular.z, max_angular_vel, min_angular_vel, + max_angular_accel, max_angular_decel); + + return dynamic_window; +} + +/** + * @brief Apply regulated linear velocity to the dynamic window + * @param regulated_linear_vel Regulated linear velocity + * @param dynamic_window Dynamic window to be regulated + */ +inline void applyRegulationToDynamicWindow( + const double & regulated_linear_vel, + DynamicWindowBounds & dynamic_window) +{ + // Create regulated bounds [0, v_reg] or [v_reg, 0] + double v_reg_min = std::min(0.0, regulated_linear_vel); + double v_reg_max = std::max(0.0, regulated_linear_vel); + + // Intersect the dynamic window with the regulated bounds + dynamic_window.min_linear_vel = std::max(dynamic_window.min_linear_vel, v_reg_min); + dynamic_window.max_linear_vel = std::min(dynamic_window.max_linear_vel, v_reg_max); + + // If min > max, collapse to the nearest boundary + if (dynamic_window.min_linear_vel > dynamic_window.max_linear_vel) { + if (dynamic_window.min_linear_vel > v_reg_max) { + dynamic_window.max_linear_vel = dynamic_window.min_linear_vel; + } else { + dynamic_window.min_linear_vel = dynamic_window.max_linear_vel; + } + } +} + +/** + * @brief Compute the optimal velocity to follow the path within the dynamic window + * @param dynamic_window Dynamic window defining feasible velocity bounds + * @param curvature Curvature of the path to follow + * @param sign Velocity sign (forward or backward) + * @return Optimal linear and angular velocity + */ +inline std::tuple computeOptimalVelocityWithinDynamicWindow( + const DynamicWindowBounds & dynamic_window, + const double & curvature, + const double & sign +) +{ + double optimal_linear_vel; + double optimal_angular_vel; + + // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) + // Select the closest point to the line + // angular_vel = curvature * linear_vel within the dynamic window. + // If multiple points are equally close, select the one with the largest linear_vel. + + // When curvature == 0, the line is angular_vel = 0 + if (std::abs(curvature) < 1e-3) { + // linear velocity + if (sign >= 0.0) { + // If moving forward, select the max linear vel + optimal_linear_vel = dynamic_window.max_linear_vel; + } else { + // If moving backward, select the min linear vel + optimal_linear_vel = dynamic_window.min_linear_vel; + } + + // angular velocity + // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 + if (dynamic_window.min_angular_vel <= 0.0 && 0.0 <= dynamic_window.max_angular_vel) { + optimal_angular_vel = 0.0; + } else { + // If not, select angular vel within dynamic window closest to 0 + if (std::abs(dynamic_window.min_angular_vel) <= std::abs(dynamic_window.max_angular_vel)) { + optimal_angular_vel = dynamic_window.min_angular_vel; + } else { + optimal_angular_vel = dynamic_window.max_angular_vel; + } + } + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, + // select the intersection point that yields the highest linear velocity. + + // List the four candidate intersection points + std::pair candidates[] = { + {dynamic_window.min_linear_vel, curvature * dynamic_window.min_linear_vel}, + {dynamic_window.max_linear_vel, curvature * dynamic_window.max_linear_vel}, + {dynamic_window.min_angular_vel / curvature, dynamic_window.min_angular_vel}, + {dynamic_window.max_angular_vel / curvature, dynamic_window.max_angular_vel} + }; + + double best_linear_vel = -std::numeric_limits::max() * sign; + double best_angular_vel = 0.0; + + for (auto [linear_vel, angular_vel] : candidates) { + // Check whether the candidate lies within the dynamic window + if (linear_vel >= dynamic_window.min_linear_vel && + linear_vel <= dynamic_window.max_linear_vel && + angular_vel >= dynamic_window.min_angular_vel && + angular_vel <= dynamic_window.max_angular_vel) + { + // Select the candidate with the largest linear velocity (considering moving direction) + if (linear_vel * sign > best_linear_vel * sign) { + best_linear_vel = linear_vel; + best_angular_vel = angular_vel; + } + } + } + + // If best_linear_vel was updated, it means that a valid intersection exists + if (best_linear_vel != -std::numeric_limits::max() * sign) { + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, + // select the point within the dynamic window that is closest to the line. + + // Because the dynamic window is a convex region, + // the closest point must be one of its four corners. + const std::array, 4> corners = { + { + {dynamic_window.min_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.min_linear_vel, dynamic_window.max_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.max_angular_vel} + }}; + + // Compute the distance from a point (linear_vel, angular_vel) + // to the line angular_vel = curvature * linear_vel + const double denom = std::sqrt(curvature * curvature + 1.0); + auto compute_dist = [&](const std::array & corner) -> double { + return std::abs(curvature * corner[0] - corner[1]) / denom; + }; + + double closest_dist = std::numeric_limits::max(); + best_linear_vel = -std::numeric_limits::max() * sign; + best_angular_vel = 0.0; + + for (const auto & corner : corners) { + const double dist = compute_dist(corner); + // Update if this corner is closer to the line, + // or equally close but has a larger linear velocity (considering moving direction) + if (dist < closest_dist || + (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) + { + closest_dist = dist; + best_linear_vel = corner[0]; + best_angular_vel = corner[1]; + } + } + + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); +} + +/** + * @brief Compute velocity commands using Dynamic Window Pure Pursuit method + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum allowable linear velocity + * @param min_linear_vel Minimum allowable linear velocity + * @param max_angular_vel Maximum allowable angular velocity + * @param min_angular_vel Minimum allowable angular velocity + * @param max_linear_accel Maximum allowable linear acceleration + * @param max_linear_decel Maximum allowable linear deceleration + * @param max_angular_accel Maximum allowable angular acceleration + * @param max_angular_decel Maximum allowable angular deceleration + * @param regulated_linear_vel Regulated linear velocity + * @param curvature Curvature of the path to follow + * @param sign Velocity sign (forward or backward) + * @param dt Control duration + * @return Optimal linear and angular velocity + */ +inline std::tuple computeDynamicWindowVelocities( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & regulated_linear_vel, + const double & curvature, + const double & sign, + const double & dt +) +{ + // compute Dynamic Window + DynamicWindowBounds dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, dt); + + // apply regulation to Dynamic Window + applyRegulationToDynamicWindow(regulated_linear_vel, dynamic_window); + + // compute optimal velocity within Dynamic Window + auto [linear_vel, angular_vel] = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + + return std::make_tuple(linear_vel, angular_vel); +} + +} // namespace dynamic_window_pure_pursuit + +} // namespace easynav + +#endif // EASYNAV_REGULATED_PP_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp new file mode 100644 index 00000000..babc2859 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp @@ -0,0 +1,153 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// and is a derivative work of nav2_regulated_pure_pursuit_controller, +// ported to the EasyNav plugin architecture. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Pure regulation heuristics for the Regulated Pure Pursuit controller. +/// +/// These are the same regulation terms described in: +/// S. Macenski, S. Singh, F. Martin, J. Gines, "Regulated Pure Pursuit for Robot Path Tracking", +/// Autonomous Robots, 2023 (https://arxiv.org/abs/2305.20026), ported from +/// nav2_regulated_pure_pursuit_controller. Since EasyNav controllers have no costmap available, +/// the cost-based regulation term is replaced by an equivalent one driven by the distance to the +/// nearest point in the fused point-cloud perception (see \ref easynav::RegulatedPurePursuitController). + +#ifndef EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ +#define EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ + +#include +#include +#include + +namespace easynav +{ + +namespace heuristics +{ + +/// \brief Curvature of the arc from the robot (at the local origin, facing +x) to a lookahead point. +/// \param lookahead_x X coordinate of the lookahead point in the robot frame. +/// \param lookahead_y Y coordinate of the lookahead point in the robot frame. +/// \return Signed curvature (1 / turning radius) of the arc. +inline double calculateCurvature(double lookahead_x, double lookahead_y) +{ + const double carrot_dist2 = lookahead_x * lookahead_x + lookahead_y * lookahead_y; + if (carrot_dist2 > 0.001) { + return 2.0 * lookahead_y / carrot_dist2; + } else { + return 0.0; + } +} + +/// \brief Apply curvature-based regulation on the linear velocity. +/// +/// Slows the robot down when the path curvature radius is smaller than \p min_radius, +/// so sharp turns are taken more conservatively. +/// \param raw_linear_vel Desired linear velocity before regulation. +/// \param curvature Curvature of the path at the regulation lookahead point. +/// \param min_radius Turning radius below which regulation kicks in. +/// \return Regulated linear velocity. +inline double curvatureConstraint( + const double raw_linear_vel, const double curvature, const double min_radius) +{ + const double radius = std::fabs(1.0 / curvature); + if (radius < min_radius) { + return raw_linear_vel * (1.0 - (std::fabs(radius - min_radius) / min_radius)); + } else { + return raw_linear_vel; + } +} + +/// \brief Apply obstacle-proximity regulation on the linear velocity. +/// +/// EasyNav controllers do not have access to a costmap, so this heuristic replaces +/// nav2_regulated_pure_pursuit_controller's cost-based regulation with an equivalent term based on +/// the distance to the closest obstacle point observed by the robot's perception. +/// \param raw_linear_vel Desired linear velocity before regulation. +/// \param min_obstacle_distance Distance (m) to the closest known obstacle ahead of the robot. +/// \param obstacle_scaling_dist Distance below which the regulation is triggered. +/// \param obstacle_scaling_gain Gain (<= 1.0) applied when scaling down the velocity. +/// \return Regulated linear velocity. +inline double obstacleConstraint( + const double raw_linear_vel, + const double min_obstacle_distance, + const double obstacle_scaling_dist, + const double obstacle_scaling_gain) +{ + if (!std::isfinite(min_obstacle_distance)) { + return raw_linear_vel; + } + + if (min_obstacle_distance < obstacle_scaling_dist) { + const double clamped_dist = std::max(0.0, min_obstacle_distance); + return raw_linear_vel * (obstacle_scaling_gain * clamped_dist / obstacle_scaling_dist); + } + + return raw_linear_vel; +} + +/// \brief Compute the scale factor to apply for linear velocity regulation on approach to goal. +/// \param remaining_path_distance Integrated distance left to travel along the path. +/// \param euclidean_dist_to_goal Straight-line distance from the robot to the goal. +/// \param approach_velocity_scaling_dist Distance away from goal at which to apply the heuristic. +/// \return A scale factor in [0.0, 1.0]. +inline double approachVelocityScalingFactor( + const double remaining_path_distance, + const double euclidean_dist_to_goal, + const double approach_velocity_scaling_dist) +{ + // Waiting to apply the threshold based on integrated distance ensures we don't erroneously + // apply approach scaling on curvy paths still far from the goal in a straight line. + if (remaining_path_distance < approach_velocity_scaling_dist) { + return euclidean_dist_to_goal / approach_velocity_scaling_dist; + } else { + return 1.0; + } +} + +/// \brief Velocity on approach to goal heuristic regulation term. +/// \param constrained_linear_vel Linear velocity already constrained by other heuristics. +/// \param remaining_path_distance Integrated distance left to travel along the path. +/// \param euclidean_dist_to_goal Straight-line distance from the robot to the goal. +/// \param min_approach_velocity Minimum velocity to use on approach to goal. +/// \param approach_velocity_scaling_dist Distance away from goal at which to apply the heuristic. +/// \return Velocity after regulation via approach-to-goal slow-down. +inline double approachVelocityConstraint( + const double constrained_linear_vel, + const double remaining_path_distance, + const double euclidean_dist_to_goal, + const double min_approach_velocity, + const double approach_velocity_scaling_dist) +{ + const double velocity_scaling = approachVelocityScalingFactor( + remaining_path_distance, euclidean_dist_to_goal, approach_velocity_scaling_dist); + double approach_vel = constrained_linear_vel * velocity_scaling; + + if (approach_vel < min_approach_velocity) { + approach_vel = min_approach_velocity; + } + + return std::min(constrained_linear_vel, approach_vel); +} + +} // namespace heuristics + +} // namespace easynav + +#endif // EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ diff --git a/controllers/easynav_regulated_pp_controller/package.xml b/controllers/easynav_regulated_pp_controller/package.xml new file mode 100644 index 00000000..7aa5f5f9 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/package.xml @@ -0,0 +1,35 @@ + + + + easynav_regulated_pp_controller + 0.4.0 + Easy Navigation: port of the Nav2 Regulated Pure Pursuit Controller. + Francisco Martín Rico + Apache-2.0 + + Steve Macenski + Shrijit Singh + + ament_cmake + + easynav_common + easynav_core + easynav_system + easynav_sensors + pluginlib + tf2 + tf2_geometry_msgs + tf2_ros + geometry_msgs + nav_msgs + std_msgs + + rclcpp_lifecycle + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp new file mode 100644 index 00000000..7d78d1a5 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -0,0 +1,542 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// and is a derivative work of nav2_regulated_pure_pursuit_controller, +// ported to the EasyNav plugin architecture. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Implementation of the RegulatedPurePursuitController class. + +#include +#include +#include + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp" +#include "easynav_regulated_pp_controller/regulation_functions.hpp" +#include "easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp" + +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_system/GoalManager.hpp" + +namespace easynav +{ + +namespace +{ +double normalizeAngle(double angle) +{ + while (angle > M_PI) {angle -= 2.0 * M_PI;} + while (angle < -M_PI) {angle += 2.0 * M_PI;} + return angle; +} +} // namespace + +RegulatedPurePursuitController::RegulatedPurePursuitController() {} + +RegulatedPurePursuitController::~RegulatedPurePursuitController() = default; + +void +RegulatedPurePursuitController::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + auto declare_and_get = [&node, &plugin_name](const std::string & name, auto & value) { + node->declare_parameter(plugin_name + "." + name, value); + node->get_parameter(plugin_name + "." + name, value); + }; + + declare_and_get("lookahead_dist", lookahead_dist_); + declare_and_get("min_lookahead_dist", min_lookahead_dist_); + declare_and_get("max_lookahead_dist", max_lookahead_dist_); + declare_and_get("lookahead_time", lookahead_time_); + declare_and_get("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_); + + declare_and_get("max_linear_vel", max_linear_vel_); + declare_and_get("min_linear_vel", min_linear_vel_); + declare_and_get("max_angular_vel", max_angular_vel_); + declare_and_get("min_angular_vel", min_angular_vel_); + declare_and_get("max_linear_accel", max_linear_accel_); + declare_and_get("max_linear_decel", max_linear_decel_); + declare_and_get("max_angular_accel", max_angular_accel_); + declare_and_get("max_angular_decel", max_angular_decel_); + declare_and_get("use_dynamic_window", use_dynamic_window_); + declare_and_get("allow_reversing", allow_reversing_); + + declare_and_get("use_rotate_to_heading", use_rotate_to_heading_); + declare_and_get("rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + declare_and_get("rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + + declare_and_get("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); + declare_and_get("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_); + declare_and_get("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_); + declare_and_get("use_fixed_curvature_lookahead", use_fixed_curvature_lookahead_); + declare_and_get("curvature_lookahead_dist", curvature_lookahead_dist_); + declare_and_get("interpolate_curvature_after_goal", interpolate_curvature_after_goal_); + + declare_and_get( + "use_obstacle_regulated_linear_velocity_scaling", + use_obstacle_regulated_linear_velocity_scaling_); + declare_and_get("obstacle_scaling_dist", obstacle_scaling_dist_); + declare_and_get("obstacle_scaling_gain", obstacle_scaling_gain_); + + declare_and_get("min_approach_linear_velocity", min_approach_linear_velocity_); + declare_and_get("approach_velocity_scaling_dist", approach_velocity_scaling_dist_); + + declare_and_get("xy_goal_tolerance", xy_goal_tolerance_); + declare_and_get("yaw_goal_tolerance", yaw_goal_tolerance_); + + lookahead_point_pub_ = node->create_publisher( + "lookahead_point", 10); + curvature_lookahead_point_pub_ = node->create_publisher( + "curvature_lookahead_point", 10); + is_rotating_to_heading_pub_ = node->create_publisher( + "is_rotating_to_heading", 10); + + last_update_ts_ = node->now(); +} + +void +RegulatedPurePursuitController::stop(NavState & nav_state, const std_msgs::msg::Header & header) +{ + cmd_vel_.header = header; + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; + last_linear_vel_ = 0.0; + last_angular_vel_ = 0.0; + is_rotating_to_heading_ = false; + last_update_ts_ = get_node()->now(); + nav_state.set("cmd_vel", cmd_vel_); +} + +double +RegulatedPurePursuitController::getLookAheadDistance(double linear_vel) const +{ + double dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) { + dist = std::fabs(linear_vel) * lookahead_time_; + dist = std::clamp(dist, min_lookahead_dist_, max_lookahead_dist_); + } + return dist; +} + +geometry_msgs::msg::Point +RegulatedPurePursuitController::circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r, + const geometry_msgs::msg::Point & center) +{ + const double x1 = p1.x - center.x; + const double y1 = p1.y - center.y; + const double x2 = p2.x - center.x; + const double y2 = p2.y - center.y; + + const double dx = x2 - x1; + const double dy = y2 - y1; + const double dr2 = dx * dx + dy * dy; + + geometry_msgs::msg::Point result; + + if (dr2 < 1e-9) { + result = p2; + return result; + } + + const double D = x1 * y2 - x2 * y1; + const double discriminant = r * r * dr2 - D * D; + + if (discriminant < 0.0) { + // No real intersection (can happen with degenerate/noisy paths): fall back to p2. + result = p2; + return result; + } + + const double sqrt_term = std::sqrt(discriminant); + const double sign_dy = (dy < 0.0) ? -1.0 : 1.0; + + const double sol1_x = (D * dy + sign_dy * dx * sqrt_term) / dr2; + const double sol1_y = (-D * dx + std::fabs(dy) * sqrt_term) / dr2; + const double sol2_x = (D * dy - sign_dy * dx * sqrt_term) / dr2; + const double sol2_y = (-D * dx - std::fabs(dy) * sqrt_term) / dr2; + + // Choose the solution further along the segment direction (closer to p2). + const double t1 = ((sol1_x - x1) * dx + (sol1_y - y1) * dy) / dr2; + const double t2 = ((sol2_x - x1) * dx + (sol2_y - y1) * dy) / dr2; + + if (t1 >= t2) { + result.x = sol1_x + center.x; + result.y = sol1_y + center.y; + } else { + result.x = sol2_x + center.x; + result.y = sol2_y + center.y; + } + result.z = 0.0; + return result; +} + +std::size_t +RegulatedPurePursuitController::findClosestPoseIndex( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position) +{ + std::size_t closest_idx = 0; + double closest_dist = std::numeric_limits::max(); + for (std::size_t i = 0; i < path.poses.size(); ++i) { + const auto & p = path.poses[i].pose.position; + const double d = std::hypot(p.x - robot_position.x, p.y - robot_position.y); + if (d < closest_dist) { + closest_dist = d; + closest_idx = i; + } + } + return closest_idx; +} + +geometry_msgs::msg::Point +RegulatedPurePursuitController::getLookAheadPoint( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position, + double lookahead_dist, + bool interpolate_after_end) +{ + auto dist_to_robot = [&robot_position](const geometry_msgs::msg::Point & p) { + return std::hypot(p.x - robot_position.x, p.y - robot_position.y); + }; + + // Only look forward from the pose closest to the robot: the controller receives the full, + // un-pruned global path, so searching from index 0 would (incorrectly) match the path's start + // pose as soon as the robot has travelled more than a lookahead distance away from it, freezing + // the carrot behind the robot for the rest of the path. + const std::size_t start_idx = findClosestPoseIndex(path, robot_position); + const auto begin_it = path.poses.begin() + static_cast(start_idx); + + auto it = std::find_if( + begin_it, path.poses.end(), + [&](const geometry_msgs::msg::PoseStamped & ps) { + return dist_to_robot(ps.pose.position) >= lookahead_dist; + }); + + if (it == path.poses.end()) { + const auto & goal = path.poses.back().pose.position; + if (interpolate_after_end && path.poses.size() >= 2 && dist_to_robot(goal) < lookahead_dist) { + const auto & prev = path.poses[path.poses.size() - 2].pose.position; + const double seg_len = std::hypot(goal.x - prev.x, goal.y - prev.y); + if (seg_len > 1e-6) { + geometry_msgs::msg::Point extended; + extended.x = goal.x + (goal.x - prev.x) / seg_len * lookahead_dist; + extended.y = goal.y + (goal.y - prev.y) / seg_len * lookahead_dist; + return circleSegmentIntersection(goal, extended, lookahead_dist, robot_position); + } + } + return goal; + } + + if (it == begin_it) { + return it->pose.position; + } + + const auto prev_it = std::prev(it); + return circleSegmentIntersection( + prev_it->pose.position, it->pose.position, lookahead_dist, robot_position); +} + +geometry_msgs::msg::Point +RegulatedPurePursuitController::toRobotFrame( + const geometry_msgs::msg::Point & point, + const geometry_msgs::msg::Pose & robot_pose, + double robot_yaw) +{ + const double dx = point.x - robot_pose.position.x; + const double dy = point.y - robot_pose.position.y; + const double c = std::cos(-robot_yaw); + const double s = std::sin(-robot_yaw); + + geometry_msgs::msg::Point local; + local.x = c * dx - s * dy; + local.y = s * dx + c * dy; + local.z = 0.0; + return local; +} + +bool +RegulatedPurePursuitController::shouldRotateToPath(double angle_to_path) const +{ + return use_rotate_to_heading_ && std::fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +void +RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, double angle_to_target, double dt) const +{ + linear_vel = 0.0; + const double sign = angle_to_target > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + // Braking (magnitude decreasing) is bounded by max_angular_decel_, speeding up + // (magnitude increasing) by max_angular_accel_; which bound applies to which side + // of the window depends on the current direction of rotation. + const double decel_bound = last_angular_vel_ >= 0.0 ? max_angular_decel_ : max_angular_accel_; + const double accel_bound = last_angular_vel_ >= 0.0 ? max_angular_accel_ : max_angular_decel_; + const double min_feasible = last_angular_vel_ - decel_bound * dt; + const double max_feasible = last_angular_vel_ + accel_bound * dt; + angular_vel = std::clamp(angular_vel, min_feasible, max_feasible); + + // Slow down to avoid overshooting the target angle. + const double decel = std::max(max_angular_decel_, 1e-9); + const double max_vel_to_stop = std::sqrt(2.0 * decel * std::fabs(angle_to_target)); + if (std::fabs(angular_vel) > max_vel_to_stop) { + angular_vel = sign * max_vel_to_stop; + } +} + +void +RegulatedPurePursuitController::applyConstraints( + double curvature, + double min_obstacle_distance, + double remaining_path_distance, + double euclidean_dist_to_goal, + double & linear_vel) const +{ + double curvature_vel = linear_vel; + double obstacle_vel = linear_vel; + + if (use_regulated_linear_velocity_scaling_) { + curvature_vel = heuristics::curvatureConstraint( + linear_vel, curvature, regulated_linear_scaling_min_radius_); + } + + if (use_obstacle_regulated_linear_velocity_scaling_) { + obstacle_vel = heuristics::obstacleConstraint( + linear_vel, min_obstacle_distance, obstacle_scaling_dist_, obstacle_scaling_gain_); + } + + linear_vel = std::min(curvature_vel, obstacle_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + linear_vel = heuristics::approachVelocityConstraint( + linear_vel, remaining_path_distance, euclidean_dist_to_goal, + min_approach_linear_velocity_, approach_velocity_scaling_dist_); +} + +double +RegulatedPurePursuitController::computeMinObstacleDistance( + NavState & nav_state, double max_range) const +{ + const auto & perceptions = nav_state.get_by_type(); + if (perceptions.empty()) { + return std::numeric_limits::infinity(); + } + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const auto & robot_frame = tf_info.robot_frame; + + auto view = PointPerceptionsOpsView(perceptions); + view.fuse(robot_frame) + .filter( + {0.0, -robot_radius_ - safety_margin_, z_min_filter_}, + {std::max(max_range, 0.0), robot_radius_ + safety_margin_, robot_height_}); + + const auto & cloud = view.as_points(); + + double min_dist = std::numeric_limits::infinity(); + for (const auto & p : cloud.points) { + if (!std::isfinite(p.x) || !std::isfinite(p.y)) {continue;} + const double d = std::hypot(p.x, p.y) - robot_radius_; + if (d < min_dist) {min_dist = d;} + } + + return min_dist; +} + +double +RegulatedPurePursuitController::remainingPathDistance( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position) +{ + if (path.poses.empty()) {return 0.0;} + + const std::size_t closest_idx = findClosestPoseIndex(path, robot_position); + const auto & closest_p = path.poses[closest_idx].pose.position; + double distance = std::hypot(closest_p.x - robot_position.x, closest_p.y - robot_position.y); + for (std::size_t i = closest_idx + 1; i < path.poses.size(); ++i) { + const auto & prev = path.poses[i - 1].pose.position; + const auto & cur = path.poses[i].pose.position; + distance += std::hypot(cur.x - prev.x, cur.y - prev.y); + } + + return distance; +} + +void +RegulatedPurePursuitController::publishCarrot( + const rclcpp::Publisher::SharedPtr & pub, + const geometry_msgs::msg::Point & carrot, + const std_msgs::msg::Header & header) const +{ + if (!pub || pub->get_subscription_count() == 0) {return;} + geometry_msgs::msg::PointStamped msg; + msg.header = header; + msg.point = carrot; + msg.point.z = 0.05; + pub->publish(msg); +} + +void +RegulatedPurePursuitController::update_rt(NavState & nav_state) +{ + if (nav_state.has("navigation_state")) { + const auto goal_state = nav_state.get("navigation_state"); + if (goal_state == easynav::GoalManager::State::IDLE) { + std_msgs::msg::Header header; + header.stamp = get_node()->now(); + stop(nav_state, header); + return; + } + } + + if (!nav_state.has("path") || !nav_state.has("robot_pose")) {return;} + + const auto & path = nav_state.get("path"); + + std_msgs::msg::Header header; + header.frame_id = path.header.frame_id; + header.stamp = get_node()->now(); + + if (path.poses.empty()) { + stop(nav_state, header); + return; + } + + const auto & robot_pose = nav_state.get("robot_pose").pose.pose; + const double robot_yaw = tf2::getYaw(robot_pose.orientation); + + const auto & goal_pose = path.poses.back().pose; + + double xy_tol = xy_goal_tolerance_; + double yaw_tol = yaw_goal_tolerance_; + if (nav_state.has("goal_tolerance.position")) { + xy_tol = nav_state.get("goal_tolerance.position"); + } + if (nav_state.has("goal_tolerance.yaw")) { + yaw_tol = nav_state.get("goal_tolerance.yaw"); + } + + const double dist_to_goal = std::hypot( + goal_pose.position.x - robot_pose.position.x, + goal_pose.position.y - robot_pose.position.y); + const double yaw_goal = tf2::getYaw(goal_pose.orientation); + const double e_theta_goal = normalizeAngle(yaw_goal - robot_yaw); + + const bool at_goal_xy = dist_to_goal <= xy_tol; + + if (at_goal_xy && std::fabs(e_theta_goal) <= yaw_tol) { + stop(nav_state, header); + return; + } + + double dt = (get_node()->now() - last_update_ts_).seconds(); + if (dt <= 0.0 || dt > 1.0) {dt = 0.05;} + last_update_ts_ = get_node()->now(); + + double linear_vel = max_linear_vel_; + double angular_vel = 0.0; + + if (use_rotate_to_heading_ && at_goal_xy) { + is_rotating_to_heading_ = true; + rotateToHeading(linear_vel, angular_vel, e_theta_goal, dt); + } else { + const double lookahead_dist = getLookAheadDistance(last_linear_vel_); + const auto carrot_global = getLookAheadPoint(path, robot_pose.position, lookahead_dist); + const auto carrot_local = toRobotFrame(carrot_global, robot_pose, robot_yaw); + publishCarrot(lookahead_point_pub_, carrot_global, header); + + auto curvature_local = carrot_local; + if (use_fixed_curvature_lookahead_) { + const auto curvature_carrot_global = getLookAheadPoint( + path, robot_pose.position, curvature_lookahead_dist_, interpolate_curvature_after_goal_); + curvature_local = toRobotFrame(curvature_carrot_global, robot_pose, robot_yaw); + publishCarrot(curvature_lookahead_point_pub_, curvature_carrot_global, header); + } + + double x_vel_sign = 1.0; + if (allow_reversing_) { + x_vel_sign = carrot_local.x >= 0.0 ? 1.0 : -1.0; + } + + double angle_to_path = std::atan2(curvature_local.y, curvature_local.x); + if (x_vel_sign < 0.0) { + angle_to_path = normalizeAngle(angle_to_path + M_PI); + } + + const double regulation_curvature = heuristics::calculateCurvature( + curvature_local.x, curvature_local.y); + + if (shouldRotateToPath(angle_to_path)) { + is_rotating_to_heading_ = true; + rotateToHeading(linear_vel, angular_vel, angle_to_path, dt); + } else { + is_rotating_to_heading_ = false; + + const double min_obstacle_distance = use_obstacle_regulated_linear_velocity_scaling_ ? + computeMinObstacleDistance(nav_state, lookahead_dist) : + std::numeric_limits::infinity(); + const double remaining_path_distance = remainingPathDistance(path, robot_pose.position); + + applyConstraints( + regulation_curvature, min_obstacle_distance, remaining_path_distance, dist_to_goal, + linear_vel); + linear_vel = x_vel_sign * std::clamp(std::fabs(linear_vel), 0.0, max_linear_vel_); + + if (!use_dynamic_window_) { + angular_vel = linear_vel * regulation_curvature; + } else { + geometry_msgs::msg::Twist current_speed; + current_speed.linear.x = last_linear_vel_; + current_speed.angular.z = last_angular_vel_; + std::tie(linear_vel, angular_vel) = + dynamic_window_pure_pursuit::computeDynamicWindowVelocities( + current_speed, max_linear_vel_, min_linear_vel_, max_angular_vel_, min_angular_vel_, + max_linear_accel_, max_linear_decel_, max_angular_accel_, max_angular_decel_, + linear_vel, regulation_curvature, x_vel_sign, dt); + } + } + } + + last_linear_vel_ = linear_vel; + last_angular_vel_ = angular_vel; + + if (is_rotating_to_heading_pub_ && is_rotating_to_heading_pub_->get_subscription_count() > 0) { + std_msgs::msg::Bool msg; + msg.data = is_rotating_to_heading_; + is_rotating_to_heading_pub_->publish(msg); + } + + cmd_vel_.header = header; + cmd_vel_.twist.linear.x = linear_vel; + cmd_vel_.twist.angular.z = angular_vel; + nav_state.set("cmd_vel", cmd_vel_); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS( + easynav::RegulatedPurePursuitController, easynav::ControllerMethodBase) diff --git a/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt new file mode 100644 index 00000000..7c7c867f --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(regulated_pp_controller_tests regulated_pp_controller_tests.cpp) +target_link_libraries(regulated_pp_controller_tests + ${PROJECT_NAME} +) diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp new file mode 100644 index 00000000..e77fb865 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp @@ -0,0 +1,257 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "easynav_regulated_pp_controller/regulation_functions.hpp" +#include "easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp" +#include "easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp" + +using easynav::heuristics::approachVelocityConstraint; +using easynav::heuristics::calculateCurvature; +using easynav::heuristics::curvatureConstraint; +using easynav::heuristics::obstacleConstraint; + +TEST(RegulationFunctions, CalculateCurvatureStraightAhead) +{ + // A lookahead point directly ahead has zero lateral offset -> zero curvature. + EXPECT_NEAR(calculateCurvature(1.0, 0.0), 0.0, 1e-6); +} + +TEST(RegulationFunctions, CalculateCurvatureLateralOffset) +{ + // k = 2y / (x^2+y^2) + const double k = calculateCurvature(1.0, 0.5); + EXPECT_NEAR(k, 2.0 * 0.5 / (1.0 * 1.0 + 0.5 * 0.5), 1e-9); +} + +TEST(RegulationFunctions, CalculateCurvatureDegenerate) +{ + EXPECT_NEAR(calculateCurvature(0.0, 0.0), 0.0, 1e-9); +} + +TEST(RegulationFunctions, CurvatureConstraintNoRegulationOnGentleTurn) +{ + // Radius = 1/curvature = 10 m, well above the 0.9 m min radius: no regulation applied. + const double v = curvatureConstraint(1.0, 0.1, 0.9); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, CurvatureConstraintSlowsOnSharpTurn) +{ + // Radius = 1/2.0 = 0.5 m, below the 1.0 m min radius: velocity should be reduced. + const double v = curvatureConstraint(1.0, 2.0, 1.0); + EXPECT_LT(v, 1.0); + EXPECT_GT(v, 0.0); +} + +TEST(RegulationFunctions, ObstacleConstraintNoObstacleNearby) +{ + const double v = obstacleConstraint(1.0, 5.0, 0.3, 1.0); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, ObstacleConstraintSlowsNearObstacle) +{ + const double v = obstacleConstraint(1.0, 0.1, 0.3, 1.0); + EXPECT_LT(v, 1.0); + EXPECT_GT(v, 0.0); +} + +TEST(RegulationFunctions, ObstacleConstraintIgnoresInfiniteDistance) +{ + const double v = obstacleConstraint(1.0, std::numeric_limits::infinity(), 0.3, 1.0); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, ApproachVelocityConstraintFarFromGoal) +{ + const double v = approachVelocityConstraint(1.0, 10.0, 10.0, 0.05, 1.0); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, ApproachVelocityConstraintNearGoal) +{ + const double v = approachVelocityConstraint(1.0, 0.5, 0.5, 0.05, 1.0); + EXPECT_LT(v, 1.0); + EXPECT_GE(v, 0.05); +} + +TEST(DynamicWindowPurePursuit, ComputeDynamicWindowClampsToAccelLimits) +{ + geometry_msgs::msg::Twist current_speed; + current_speed.linear.x = 0.0; + current_speed.angular.z = 0.0; + + const auto window = easynav::dynamic_window_pure_pursuit::computeDynamicWindow( + current_speed, /*max_linear_vel=*/ 1.0, /*min_linear_vel=*/ -1.0, + /*max_angular_vel=*/ 1.0, /*min_angular_vel=*/ -1.0, + /*max_linear_accel=*/ 2.0, /*max_linear_decel=*/ 2.0, + /*max_angular_accel=*/ 2.0, /*max_angular_decel=*/ 2.0, /*dt=*/ 0.1); + + EXPECT_NEAR(window.max_linear_vel, 0.2, 1e-9); + EXPECT_NEAR(window.min_linear_vel, -0.2, 1e-9); +} + +TEST(DynamicWindowPurePursuit, ComputeOptimalVelocityZeroCurvatureForward) +{ + easynav::dynamic_window_pure_pursuit::DynamicWindowBounds window; + window.max_linear_vel = 1.0; + window.min_linear_vel = -1.0; + window.max_angular_vel = 1.0; + window.min_angular_vel = -1.0; + + const auto [lin, ang] = + easynav::dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( + window, /*curvature=*/ 0.0, /*sign=*/ 1.0); + + EXPECT_NEAR(lin, 1.0, 1e-9); + EXPECT_NEAR(ang, 0.0, 1e-9); +} + +class FriendRegulatedPurePursuitController : public easynav::RegulatedPurePursuitController +{ +public: + using easynav::RegulatedPurePursuitController::getLookAheadPoint; + using easynav::RegulatedPurePursuitController::circleSegmentIntersection; + using easynav::RegulatedPurePursuitController::toRobotFrame; + using easynav::RegulatedPurePursuitController::remainingPathDistance; + using easynav::RegulatedPurePursuitController::findClosestPoseIndex; +}; + +TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointInterpolatesOnSegment) +{ + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped p0, p1, p2; + p0.pose.position.x = 0.0; + p1.pose.position.x = 1.0; + p2.pose.position.x = 2.0; + path.poses = {p0, p1, p2}; + + geometry_msgs::msg::Point robot; + robot.x = 0.0; + robot.y = 0.0; + + const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( + path, robot, /*lookahead_dist=*/ 1.5); + + EXPECT_NEAR(carrot.x, 1.5, 1e-6); + EXPECT_NEAR(carrot.y, 0.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointIgnoresPathBehindRobot) +{ + // Regression test: the controller receives the full, un-pruned global path, so the carrot + // search must start from the pose closest to the robot, not from the path's start pose. A path + // start pose that is far behind the robot must never be picked up as the carrot. + nav_msgs::msg::Path path; + for (double x = 0.0; x <= 10.0; x += 0.1) { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x; + path.poses.push_back(p); + } + + // Robot has already travelled most of the path. + geometry_msgs::msg::Point robot; + robot.x = 9.0; + robot.y = 0.0; + + const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( + path, robot, /*lookahead_dist=*/ 0.5); + + // The carrot must be ahead of the robot (close to x=9.5), never back at the path's start (x=0). + EXPECT_NEAR(carrot.x, 9.5, 1e-6); + EXPECT_NEAR(carrot.y, 0.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, FindClosestPoseIndexPicksNearestNotFirst) +{ + nav_msgs::msg::Path path; + for (double x = 0.0; x <= 10.0; x += 1.0) { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x; + path.poses.push_back(p); + } + + geometry_msgs::msg::Point robot; + robot.x = 7.1; + robot.y = 0.0; + + const auto idx = FriendRegulatedPurePursuitController::findClosestPoseIndex(path, robot); + EXPECT_EQ(idx, 7u); +} + +TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointClampsToPathEnd) +{ + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped p0, p1; + p0.pose.position.x = 0.0; + p1.pose.position.x = 1.0; + path.poses = {p0, p1}; + + geometry_msgs::msg::Point robot; + robot.x = 0.0; + robot.y = 0.0; + + const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( + path, robot, /*lookahead_dist=*/ 5.0); + + EXPECT_NEAR(carrot.x, 1.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, ToRobotFrameRotatesAndTranslates) +{ + geometry_msgs::msg::Point global_point; + global_point.x = 1.0; + global_point.y = 1.0; + + geometry_msgs::msg::Pose robot_pose; + robot_pose.position.x = 1.0; + robot_pose.position.y = 0.0; + + // Robot facing +90 degrees (looking along +y): the point straight ahead in the global + // frame (0, 1) relative to the robot should map to local +x. + const auto local = FriendRegulatedPurePursuitController::toRobotFrame( + global_point, robot_pose, M_PI_2); + + EXPECT_NEAR(local.x, 1.0, 1e-6); + EXPECT_NEAR(local.y, 0.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, RemainingPathDistanceFromClosestPoint) +{ + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped p0, p1, p2; + p0.pose.position.x = 0.0; + p1.pose.position.x = 1.0; + p2.pose.position.x = 2.0; + path.poses = {p0, p1, p2}; + + geometry_msgs::msg::Point robot; + robot.x = 1.0; + robot.y = 0.0; + + const double dist = FriendRegulatedPurePursuitController::remainingPathDistance(path, robot); + EXPECT_NEAR(dist, 1.0, 1e-6); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/controllers/easynav_serest_controller/CHANGELOG.rst b/controllers/easynav_serest_controller/CHANGELOG.rst index 0c86bc2a..d57cbc21 100644 --- a/controllers/easynav_serest_controller/CHANGELOG.rst +++ b/controllers/easynav_serest_controller/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package easynav_serest_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Sync to current EasyNavigation +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Remove C++20/C++23 features and update to new MethodBase interface +* Merge branch 'set_robot_frame' into frames-fix-pr-40 +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Sync to current EasyNavigation +* Translate comments +* Referencing base class if ot void +* Merge remote-tracking branch 'upstream/rolling' into rolling +* Fix Link error in controller +* Optimize execution +* Cleanup unused headers +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel, estherag + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index 26541250..b0b5b0e4 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_controller/CMakeLists.txt @@ -5,16 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) +find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -28,7 +28,11 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + easynav_sensors::easynav_sensors + rclcpp::rclcpp tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -66,8 +70,12 @@ pluginlib_export_plugin_description_file(easynav_core easynav_serest_controller_ ament_export_dependencies( easynav_common easynav_core + easynav_sensors + rclcpp pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs ) diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index f5d6fa49..9546f28e 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -4,20 +4,26 @@ ## Description + A SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---:| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_serest_controller/SerestController` - **Type:** `easynav::SerestController` - **Base Class:** `easynav::ControllerMethodBase` @@ -25,8 +31,12 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin - **Description:** A SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_serest_controller/SerestController/...`. +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. + | Name | Type | Default | Description | |---|---|---:|---| | `.a_acc` | `double` | `0.8` | Comfortable forward acceleration (m/s²). | @@ -68,17 +78,18 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.v_progress_min` | `double` | `0.05` | Minimum forward speed to ensure progress (m/s). | | `.v_ref` | `double` | `0.6` | Nominal reference speed (m/s). | - ## Interfaces (Topics and Services) ### Subscriptions and Publications -This controller communicates through `NavState` (no direct ROS topics in this plugin). +This controller communicates through `NavState` (no direct ROS topics in this plugin). ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `path` | `nav_msgs::msg::Path` | **Read** | Reference path. | @@ -100,9 +111,10 @@ This package does not create service servers or clients. | `serest.debug.goal.in_final_align` | `double/int` | **Write** | Debug metric | | `serest.debug.goal.arrived` | `double/int` | **Write** | Debug metric | - ## TF Frames + This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). TF is not directly used in this plugin. ## License -GPL-3.0-only + +Apache-2.0 diff --git a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp index 9a22d258..c854e88c 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -1,35 +1,28 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ -#include #include #include -#include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "tf2/utils.hpp" #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" @@ -65,9 +58,9 @@ class SerestController : public ControllerMethodBase /** * @brief Initialize parameters and internal state. - * @return std::expected Empty on success; error message otherwise. + * @throws std::runtime_error on initialization error. */ - std::expected on_initialize() override; + void on_initialize() override; /** * @brief Real-time control update (called ~20–30 Hz). @@ -123,7 +116,7 @@ class SerestController : public ControllerMethodBase { size_t seg_idx{0}; ///< Current segment index [i, i+1]. double s_star{0.0}; ///< Arc-length at the closest point. - Vec2 closest{0.0, 0.0}; ///< Closest point coordinates on the path. + Vec2 closest{0.0, 0.0}; ///< Closest point coordinates on the path. double t{0.0}; ///< Segment parameter in [0,1]. }; @@ -185,7 +178,7 @@ class SerestController : public ControllerMethodBase * @return double Minimum obstacle distance (m). Infinity if none is found. */ double closest_obstacle_distance( - const NavState & nav_state) const; + const NavState & nav_state); /** * @brief Compute a safe linear speed bound from obstacle distance and slope. @@ -229,7 +222,7 @@ class SerestController : public ControllerMethodBase void safety_limits( const NavState & nav_state, const RefKinematics & rk, - double & d_closest, double & v_safe, double & v_curv) const; + double & d_closest, double & v_safe, double & v_curv); // Aplica corner‑guard: ajusta v_prog_ref y obtiene omega_boost + término ápice. void apply_corner_guard( @@ -266,7 +259,7 @@ class SerestController : public ControllerMethodBase // --- Parameters --- /// @brief Allow reverse motion (if false, forward-only). - bool allow_reverse_{false}; + bool allow_reverse_{false}; /// @brief Minimal forward progress (m/s) when reasonably aligned. double v_progress_min_{0.05}; /// @brief Max fraction of forward progress that -k_s * e_y can cancel. @@ -328,7 +321,7 @@ class SerestController : public ControllerMethodBase double final_align_wmax_{0.6}; /// @brief Enable corner-guard behavior in tight turns. - bool corner_guard_enable_{true}; + bool corner_guard_enable_{true}; /// @brief Corner-guard weight for outside lateral error (e_y_out) in speed reduction. double corner_gain_ey_{1.5}; /// @brief Corner-guard weight for |e_theta| in speed reduction. @@ -357,6 +350,8 @@ class SerestController : public ControllerMethodBase double last_vrot_{0.0}; /// @brief Last update timestamp. rclcpp::Time last_update_ts_; + /// @brief Last input timestamp (max of path and odom). + rclcpp::Time last_input_ts_; /// @brief Output TwistStamped buffer. geometry_msgs::msg::TwistStamped twist_stamped_; }; diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 0e39b126..cd44e27d 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -2,16 +2,20 @@ easynav_serest_controller - 0.0.2 + 0.4.0 Easy Navigation: SeReST Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake easynav_common easynav_core + easynav_sensors + rclcpp pluginlib + tf2 + tf2_geometry_msgs tf2_ros geometry_msgs nav_msgs diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 669a2d2d..30ea299b 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the SimpleController class. @@ -24,8 +20,11 @@ #include #include -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_serest_controller/SerestController.hpp" #include "pluginlib/class_list_macros.hpp" @@ -37,13 +36,13 @@ namespace easynav SerestController::SerestController() = default; SerestController::~SerestController() = default; -std::expected +void SerestController::on_initialize() { auto node = get_node(); const auto & ns = get_plugin_name(); - // Máximos y límites básicos + // Maximums and basic limits node->declare_parameter(ns + ".allow_reverse", allow_reverse_); node->declare_parameter(ns + ".v_progress_min", v_progress_min_); node->declare_parameter(ns + ".k_s_share_max", k_s_share_max_); @@ -53,7 +52,7 @@ SerestController::on_initialize() node->declare_parameter(ns + ".max_linear_acc", max_linear_acc_); node->declare_parameter(ns + ".max_angular_acc", max_angular_acc_); - // Seguimiento + // Tracking node->declare_parameter(ns + ".k_s", k_s_); node->declare_parameter(ns + ".k_theta", k_theta_); node->declare_parameter(ns + ".k_y", k_y_); @@ -61,7 +60,7 @@ SerestController::on_initialize() node->declare_parameter(ns + ".v_ref", v_ref_); node->declare_parameter(ns + ".eps", eps_); - // Seguridad + // Safety node->declare_parameter(ns + ".a_acc", a_acc_); node->declare_parameter(ns + ".a_brake", a_brake_); node->declare_parameter(ns + ".a_lat_max", a_lat_max_); @@ -70,7 +69,7 @@ SerestController::on_initialize() node->declare_parameter(ns + ".d_hard", d_hard_); node->declare_parameter(ns + ".t_emerg", t_emerg_); - // Blend en vértices + // Blend at vertices node->declare_parameter(ns + ".blend_base", blend_base_); node->declare_parameter(ns + ".blend_k_per_v", blend_k_per_v_); node->declare_parameter(ns + ".kappa_max", kappa_max_); @@ -145,8 +144,6 @@ SerestController::on_initialize() last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); - - return {}; } SerestController::PathData @@ -214,31 +211,31 @@ SerestController::ref_heading_and_curvature( if (i + 1 >= pd.pts.size()) {return v2(1, 0);} return normalize(pd.pts[i + 1] - pd.pts[i]); }; - auto atan2dir = [](const Vec2 & t){return std::atan2(t.y, t.x);}; + auto atan2dir = [](const Vec2 & t) {return std::atan2(t.y, t.x);}; - // Índices de segmentos relevante: i-1, i, i + 1 + // Relevant segment indices: i-1, i, i + 1 const size_t i = prj.seg_idx; const Vec2 Ti = seg_dir(i); double psi_i = atan2dir(Ti); - // Mezcla local con previsualización + // Local blend with look-ahead double b = std::max(0.1, blend_base_ + blend_k_per_v_ * std::fabs(v_prev)); - // Determina si estamos cerca del comienzo o fin de un segmento + // Determine if we are near the beginning or end of a segment double s_i = pd.s_acc[i]; double s_ip1 = pd.s_acc[i + 1]; double s = prj.s_star; - // Por defecto: rumbo constante del segmento, kappa = 0 + // Default: constant segment heading, kappa = 0 rk.psi_ref = psi_i; rk.kappa_hat = 0.0; - // Blend cerca del inicio del segmento i (con i-1) + // Blend near the beginning of segment i (with i-1) if (i > 0 && (s - s_i) < b) { Vec2 Tim1 = seg_dir(i - 1); double psi_im1 = atan2dir(Tim1); - double w = 1.0 / (1.0 + std::exp(-( (s - s_i) / b ))); // sigmoide en [s_i, s_i+b] - // Interpolación circular de rumbos + double w = 1.0 / (1.0 + std::exp(-( (s - s_i) / b ))); // sigmoid in [s_i, s_i+b] + // Circular interpolation of headings double sx = (1.0 - w) * std::cos(psi_im1) + w * std::cos(psi_i); double sy = (1.0 - w) * std::sin(psi_im1) + w * std::sin(psi_i); rk.psi_ref = std::atan2(sy, sx); @@ -249,12 +246,12 @@ SerestController::ref_heading_and_curvature( rk.kappa_hat = std::clamp(dpsi * sigma_prime, -kappa_max_, kappa_max_); } - // Blend cerca del final del segmento i (hacia i + 1) + // Blend near the end of segment i (towards i + 1) if (i + 1 < pd.pts.size() - 1 && (s_ip1 - s) < b) { Vec2 Tip1 = seg_dir(i + 1); double psi_ip1 = atan2dir(Tip1); - double w = 1.0 / (1.0 + std::exp(-( (s_ip1 - s) / b ))); // simétrico - // Mezcla entre i y i + 1 + double w = 1.0 / (1.0 + std::exp(-( (s_ip1 - s) / b ))); // symmetric + // Blend between i and i + 1 double sx = (1.0 - w) * std::cos(psi_i) + w * std::cos(psi_ip1); double sy = (1.0 - w) * std::sin(psi_i) + w * std::sin(psi_ip1); rk.psi_ref = std::atan2(sy, sx); @@ -262,7 +259,7 @@ SerestController::ref_heading_and_curvature( double dpsi = std::atan2(std::sin(psi_ip1 - psi_i), std::cos(psi_ip1 - psi_i)); double sigma_prime = (w * (1 - w)) / b; double kappa2 = std::clamp(dpsi * sigma_prime, -kappa_max_, kappa_max_); - // Si hay dos blends solapados, combinamos suavemente (promedio) + // If there are two overlapping blends, combine smoothly (average) rk.kappa_hat = 0.5 * (rk.kappa_hat + kappa2); } @@ -275,40 +272,53 @@ SerestController::frenet_errors( const Projection & prj, double psi_ref, double & e_y, double & e_theta) const { - // Vector normal a la derecha de la tangente + // Normal vector to the right of the tangent Vec2 T = v2(std::cos(psi_ref), std::sin(psi_ref)); - Vec2 N = v2(-T.y, T.x); // 90º a la izquierda (convención) + Vec2 N = v2(-T.y, T.x); // 90º to the left (convention) Vec2 err = robot_xy - prj.closest; - e_y = dot(err, N); // distancia lateral con signo + e_y = dot(err, N); // signed lateral distance e_theta = std::atan2(std::sin(robot_yaw - psi_ref), std::cos(robot_yaw - psi_ref)); } double SerestController::closest_obstacle_distance( - const NavState & nav_state) const + const NavState & nav_state) { - // 1) Preferir medición directa si existe + // 1) Prefer direct measurement if it exists if (nav_state.has("closest_obstacle_distance")) { try { return nav_state.get("closest_obstacle_distance"); } catch (...) { - // continúa a estimación + // fall through to estimation } } - // 2) Analizar los sensores de distancia - if (!nav_state.has("points")) {return std::numeric_limits::infinity();} + // 2) Analyze distance sensors + if (!nav_state.has_group("points")) {return std::numeric_limits::infinity();} + + const auto & perceptions = nav_state.get_no_group(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - const auto perceptions = nav_state.get("points"); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.3) - .fuse(get_tf_prefix() + "base_link") - ->filter({-dist_search_radius_, -dist_search_radius_, NAN}, - {dist_search_radius_, dist_search_radius_, 2.0}) - .collapse({NAN, NAN, 0.1}) - ->downsample(0.3) - .as_points(); + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(0.3) + .fuse(tf_info.robot_frame) + .filter( + {-dist_search_radius_, -dist_search_radius_, NAN}, + {dist_search_radius_, dist_search_radius_, 2.0}) + .collapse({NAN, NAN, 0.1}) + .downsample(0.3); + const auto & fused = view.as_points(); + + { + const auto view_latest = view.get_latest_stamp(); + const rclcpp::Time view_latest_same_clock( + view_latest.nanoseconds(), + last_input_ts_.get_clock_type()); + if (last_input_ts_ < view_latest_same_clock) { + last_input_ts_ = view_latest_same_clock; + } + } double min_dist = std::numeric_limits::infinity(); for (const auto p : fused) { @@ -322,7 +332,7 @@ SerestController::closest_obstacle_distance( double SerestController::v_safe_from_distance(double d, double slope_sin) const { - // a_eff reduce la capacidad de frenado en pendiente; en 2D slope_sin = 0 + // a_eff reduces braking capability on slopes; in 2D slope_sin = 0 const double a_eff = std::max(0.1, a_brake_ - 9.81 * slope_sin); if (d <= d0_margin_) {return 0.0;} @@ -335,7 +345,7 @@ SerestController::v_safe_from_distance(double d, double slope_sin) const double SerestController::v_curvature_limit(double kappa_hat) const { - // Tres límites típicos: v_max, ω_max/|κ| y sqrt(a_lat_max/|κ|) + // Three typical limits: v_max, ω_max/|κ| and sqrt(a_lat_max/|κ|) double v1 = max_linear_speed_; double v2 = std::numeric_limits::infinity(); double v3 = std::numeric_limits::infinity(); @@ -364,14 +374,23 @@ SerestController::fetch_required_inputs( nav_msgs::msg::Path & path, nav_msgs::msg::Odometry & odom) { - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { - publish_stop(nav_state, "base_link"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map")) { + publish_stop(nav_state, tf_info.robot_footprint_frame); return false; } path = nav_state.get("path"); odom = nav_state.get("robot_pose"); + if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { + last_input_ts_ = rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()); + } + if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { + last_input_ts_ = rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()); + } + if (path.poses.empty()) { publish_stop(nav_state, path.header.frame_id); return false; @@ -399,10 +418,6 @@ SerestController::compute_goal_zone( double & stop_r, double & slow_r, double & gamma_slow, Vec2 & goal_xy, double & yaw_goal) const { - const double PI = 3.14159265358979323846; - const double goal_yaw_tol = goal_yaw_tol_deg_ * PI / 180.0; // (no se devuelve, pero útil si quieres) - (void)goal_yaw_tol; - const auto & pgoal = path.poses.back().pose.position; yaw_goal = tf2::getYaw(path.poses.back().pose.orientation); goal_xy = Vec2{pgoal.x, pgoal.y}; @@ -410,6 +425,9 @@ SerestController::compute_goal_zone( dist_xy_goal = std::hypot(robot_xy.x - goal_xy.x, robot_xy.y - goal_xy.y); e_theta_goal = std::atan2(std::sin(robot_yaw - yaw_goal), std::cos(robot_yaw - yaw_goal)); + // Use externally provided goal position tolerance when available (via update_rt), + // which stores it in goal_pos_tol_. Here we only shape the radial zone using + // the current value of goal_pos_tol_ as stop radius. stop_r = std::max(0.0, goal_pos_tol_); slow_r = std::max(slow_radius_, stop_r + 0.05); @@ -423,12 +441,12 @@ void SerestController::safety_limits( const NavState & nav_state, const RefKinematics & rk, - double & d_closest, double & v_safe, double & v_curv) const + double & d_closest, double & v_safe, double & v_curv) { d_closest = closest_obstacle_distance(nav_state); - v_safe = v_safe_from_distance(d_closest, /*slope_sin=*/0.0); + v_safe = v_safe_from_distance(d_closest, /*slope_sin=*/ 0.0); - // Límite por curvatura (versión “soft” del archivo original) + // Curvature-based limit ("soft" version of the original file) const double ak = std::fabs(rk.kappa_hat); double v_curv_soft = std::numeric_limits::infinity(); if (ak > 1e-6) { @@ -463,7 +481,7 @@ SerestController::apply_corner_guard( alpha_corner = std::clamp(1.0 / (1.0 + penal), corner_min_alpha_, 1.0); omega_boost = 1.0 + corner_boost_omega_ * std::min(1.0, e_y_out / std::max(1e-3, ell_)); - // Empuje al “ápice” si vas por fuera y hay curvatura + // Push towards the "apex" if you are outside and there is curvature if (std::fabs(rk.kappa_hat) > 1e-6 && e_y_out > 0.0) { const double e_y_des = -sgn_k * std::max(0.0, apex_ey_des_); const double e_y_err = e_y - e_y_des; @@ -478,17 +496,16 @@ SerestController::should_turn_in_place( bool allow_reverse, double e_theta, double e_theta_goal, double dist_to_end, [[maybe_unused]] double turn_in_place_thr) const { - // Mantenemos compatibilidad con la firma, pero ignoramos turn_in_place_thr - // y usamos dos umbrales internos sin exponer parámetros. - const double PI = 3.14159265358979323846; - const double thr_enter = 60.0 * PI / 180.0; // entra a girar si |e_theta| > 60° - // const double thr_exit = 35.0 * PI / 180.0; // sale de girar si |e_theta| < 35° - - // No permitimos “atajo” marcha atrás en esta decisión: si no permites reverse, - // el criterio es más estricto. + // Keep compatibility with the signature, but ignore turn_in_place_thr + // and use two internal thresholds without exposing parameters. + const double thr_enter = 60.0 * M_PI / 180.0; // enter TiP if |e_theta| > 60° + // const double thr_exit = 35.0 * PI / 180.0; // exit TiP if |e_theta| < 35° + + // Do not allow reverse "shortcut" in this decision: if reverse is not allowed, + // the criterion is stricter. bool tip = (!allow_reverse) && (std::fabs(e_theta) > thr_enter); - // Cerca del final, si el yaw al objetivo es grande, también pedimos TiP + // Near the end, if the yaw error to the goal is large, also request TiP if (dist_to_end < 0.50) { if (!allow_reverse && std::fabs(e_theta_goal) > thr_enter) { tip = true; @@ -504,8 +521,7 @@ SerestController::maybe_final_align_and_publish( double dist_xy_goal, double stop_r, double e_theta_goal, double gamma_slow, double dt) { - const double PI = 3.14159265358979323846; - const double goal_yaw_tol = goal_yaw_tol_deg_ * PI / 180.0; + const double goal_yaw_tol = goal_yaw_tol_deg_ * M_PI / 180.0; if (dist_xy_goal > stop_r) { return false; @@ -533,7 +549,7 @@ SerestController::maybe_final_align_and_publish( // Publicación y actualización de estado twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = last_input_ts_; twist_stamped_.twist.linear.x = vlin; twist_stamped_.twist.angular.z = vrot; @@ -576,7 +592,7 @@ SerestController::publish_cmd_and_debug( int in_final_align, int arrived) { twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = last_input_ts_; twist_stamped_.twist.linear.x = vlin; twist_stamped_.twist.angular.z = vrot; nav_state.set("cmd_vel", twist_stamped_); @@ -599,7 +615,7 @@ SerestController::publish_cmd_and_debug( void SerestController::publish_stop(NavState & nav_state, const std::string & frame_id) { - auto now = get_node()->now(); + auto now = last_input_ts_; twist_stamped_.header.frame_id = frame_id; twist_stamped_.header.stamp = now; twist_stamped_.twist.linear.x = 0.0; @@ -618,6 +634,16 @@ SerestController::update_rt(NavState & nav_state) nav_msgs::msg::Odometry odom; if (!fetch_required_inputs(nav_state, path, odom)) {return;} + // 1.5) Goal tolerances: prefer shared GoalManager values, fallback to local params + double goal_pos_tol = goal_pos_tol_; + double goal_yaw_tol = goal_yaw_tol_deg_ * (M_PI / 180.0); + if (nav_state.has("goal_tolerance.position")) { + goal_pos_tol = nav_state.get("goal_tolerance.position"); + } + if (nav_state.has("goal_tolerance.yaw")) { + goal_yaw_tol = nav_state.get("goal_tolerance.yaw"); + } + // 2) Robot state (position + yaw) Vec2 robot_xy; double yaw = 0.0; robot_state_from_odom(odom, robot_xy, yaw); @@ -634,8 +660,9 @@ SerestController::update_rt(NavState & nav_state) // 5) Goal-related terms and slow/stop zone shaping double dist_xy_goal = 0.0, e_theta_goal = 0.0, stop_r = 0.0, slow_r = 0.0, gamma_slow = 1.0; Vec2 goal_xy; double yaw_goal = 0.0; - compute_goal_zone(path, robot_xy, yaw, dist_xy_goal, e_theta_goal, - stop_r, slow_r, gamma_slow, goal_xy, yaw_goal); + compute_goal_zone( + path, robot_xy, yaw, dist_xy_goal, e_theta_goal, + stop_r, slow_r, gamma_slow, goal_xy, yaw_goal); // 6) Global safety limits derived from sensors and curvature double d_closest = 0.0, v_safe = 0.0, v_curv = 0.0; @@ -643,17 +670,17 @@ SerestController::update_rt(NavState & nav_state) // 6.5) Early turn-in-place with hysteresis (start-of-path aware) { - const double PI = 3.14159265358979323846; const double s_total = pd.s_acc.back(); const double dist_to_end = s_total - prj.s_star; // Internal angular thresholds (enter/exit) - const double thr_enter = 60.0 * PI / 180.0; - const double thr_exit = 35.0 * PI / 180.0; + const double thr_enter = 60.0 * M_PI / 180.0; + const double thr_exit = 35.0 * M_PI / 180.0; const double near_start_s = 0.30; // treat the first 30 cm as the start region // Base request from the regular criterion (using the provided threshold) - bool tip_request = should_turn_in_place(allow_reverse_, e_theta, e_theta_goal, dist_to_end, + bool tip_request = should_turn_in_place( + allow_reverse_, e_theta, e_theta_goal, dist_to_end, thr_enter); // Additional start-of-path gate: enforce TiP if still near s*=0 and yaw misalignment is large @@ -694,10 +721,10 @@ SerestController::update_rt(NavState & nav_state) publish_cmd_and_debug( nav_state, path, vlin, vrot, e_y, e_theta, rk.kappa_hat, - d_closest, v_safe, v_curv, /*alpha*/1.0, + d_closest, v_safe, v_curv, /*alpha*/ 1.0, allow_reverse_, dist_to_end, dist_xy_goal, gamma_slow, - /*in_final_align*/0, /*arrived*/0); + /*in_final_align*/ 0, /*arrived*/ 0); return; } } @@ -712,8 +739,7 @@ SerestController::update_rt(NavState & nav_state) double v_prog_ref = v_prog_ref_free * gamma_slow; // Maintain a small cruising speed when roughly aligned and outside the stop zone (no reverse) - const double PI = 3.14159265358979323846; - const double align_thr = 30.0 * PI / 180.0; + const double align_thr = 30.0 * M_PI / 180.0; if (!allow_reverse_ && (dist_xy_goal > stop_r) && std::fabs(e_theta) < align_thr) { v_prog_ref = std::max(v_prog_ref, std::min(slow_min_speed_, v_prog_ref_free)); } @@ -740,7 +766,7 @@ SerestController::update_rt(NavState & nav_state) // 8) Final alignment inside stop zone (publishes and returns if active) if (maybe_final_align_and_publish( - nav_state, path, dist_xy_goal, stop_r, e_theta_goal, gamma_slow, dt)) + nav_state, path, dist_xy_goal, stop_r, e_theta_goal, gamma_slow, dt)) { return; } @@ -764,12 +790,13 @@ SerestController::update_rt(NavState & nav_state) // Optional classic TiP safeguard using the same angular threshold { - const double turn_in_place_thr = (60.0 * PI / 180.0); + const double turn_in_place_thr = (60.0 * M_PI / 180.0); const double s_total = pd.s_acc.back(); const double dist_to_end = s_total - prj.s_star; - if (should_turn_in_place(allow_reverse_, e_theta, e_theta_goal, dist_to_end, - turn_in_place_thr)) + if (should_turn_in_place( + allow_reverse_, e_theta, e_theta_goal, dist_to_end, + turn_in_place_thr)) { v_cmd_raw = 0.0; omega_nom = -k_theta_ * e_theta - k_y_ * std::atan(e_y / std::max(ell_, 1e-3)); @@ -807,7 +834,7 @@ SerestController::update_rt(NavState & nav_state) d_closest, v_safe, v_curv, alpha, allow_reverse_, dist_to_end, dist_xy_goal, gamma_slow, - /*in_final_align=*/0, /*arrived=*/0); + /*in_final_align=*/ 0, /*arrived=*/ 0); } } // namespace easynav diff --git a/controllers/easynav_simple_controller/CHANGELOG.rst b/controllers/easynav_simple_controller/CHANGELOG.rst index d4d6ebc3..8b9a48ea 100644 --- a/controllers/easynav_simple_controller/CHANGELOG.rst +++ b/controllers/easynav_simple_controller/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package easynav_simple_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, estherag + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_simple_controller/CMakeLists.txt b/controllers/easynav_simple_controller/CMakeLists.txt index c3346d89..f99d627d 100644 --- a/controllers/easynav_simple_controller/CMakeLists.txt +++ b/controllers/easynav_simple_controller/CMakeLists.txt @@ -5,16 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -29,7 +28,10 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -68,8 +70,11 @@ pluginlib_export_plugin_description_file(easynav_core easynav_simple_controller_ ament_export_dependencies( easynav_common easynav_core + rclcpp pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs ) diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index c01e2eb2..43524dbc 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -6,12 +6,16 @@ Simple path-following controller that uses PID controllers and a look-ahead reference pose to follow a planned path. It produces velocity commands (`cmd_vel`) based on the reference pose sampled at a look-ahead distance and limits linear/angular speeds and accelerations. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---:| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | @@ -24,42 +28,49 @@ Simple path-following controller that uses PID controllers and a look-ahead refe - **Description:** Path-following controller using PID (linear and angular) and a look-ahead strategy. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_simple_controller/SimpleController/...`. +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. + | Name | Type | Default | Description | |---|---|---:|---| -| `max_linear_speed` | `double` | `1.0` | Maximum linear speed (m/s). -| `max_angular_speed` | `double` | `1.0` | Maximum angular speed (rad/s). -| `max_linear_acc` | `double` | `0.3` | Maximum linear acceleration (m/s²). -| `max_angular_acc` | `double` | `0.3` | Maximum angular acceleration (rad/s²). -| `look_ahead_dist` | `double` | `1.0` | Look-ahead distance to sample the reference pose on the path (m). -| `tolerance_dist` | `double` | `0.05` | Distance threshold to switch to pure orientation tracking (m). -| `final_goal_angle_tolerance` | `double` | `0.1` | Angular tolerance (rad) used to decide final-goal arrival. -| `k_rot` | `double` | `0.5` | Gain used to reduce linear speed based on angular velocity (higher: stronger reduction while turning). -| `linear_kp` | `double` | `0.95` | Proportional gain for the linear PID controller. -| `linear_ki` | `double` | `0.03` | Integral gain for the linear PID controller. -| `linear_kd` | `double` | `0.08` | Derivative gain for the linear PID controller. -| `angular_kp` | `double` | `1.5` | Proportional gain for the angular PID controller. -| `angular_ki` | `double` | `0.03` | Integral gain for the angular PID controller. -| `angular_kd` | `double` | `0.08` | Derivative gain for the angular PID controller. +| `max_linear_speed` | `double` | `1.0` | Maximum linear speed (m/s). | +| `max_angular_speed` | `double` | `1.0` | Maximum angular speed (rad/s). | +| `max_linear_acc` | `double` | `0.3` | Maximum linear acceleration (m/s²). | +| `max_angular_acc` | `double` | `0.3` | Maximum angular acceleration (rad/s²). | +| `look_ahead_dist` | `double` | `1.0` | Look-ahead distance to sample the reference pose on the path (m). | +| `tolerance_dist` | `double` | `0.05` | Distance threshold to switch to pure orientation tracking (m). | +| `final_goal_angle_tolerance` | `double` | `0.1` | Angular tolerance (rad) used to decide final-goal arrival. | +| `k_rot` | `double` | `0.5` | Gain used to reduce linear speed based on angular velocity (higher: stronger reduction while turning). | +| `linear_kp` | `double` | `0.95` | Proportional gain for the linear PID controller. | +| `linear_ki` | `double` | `0.03` | Integral gain for the linear PID controller. | +| `linear_kd` | `double` | `0.08` | Derivative gain for the linear PID controller. | +| `angular_kp` | `double` | `1.5` | Proportional gain for the angular PID controller. | +| `angular_ki` | `double` | `0.03` | Integral gain for the angular PID controller. | +| `angular_kd` | `double` | `0.08` | Derivative gain for the angular PID controller. | ## Interfaces (NavState, Topics and Services) ### NavState + This controller uses the shared `NavState` bag provided by the `easynav_core` framework. The following keys are used at runtime by `SimpleController`: | Key | Type | Access | Notes | |---|---|---|---| -| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot odometry used to compute the robot pose and yaw. -| `path` | `nav_msgs::msg::Path` | **Read** | Planned path to follow. The controller samples a reference pose at `look_ahead_dist` along this path. -| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command. Header.frame_id is set to `path.header.frame_id` when available and stamp to the controller node clock. +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot odometry used to compute the robot pose and yaw. | +| `path` | `nav_msgs::msg::Path` | **Read** | Planned path to follow. The controller samples a reference pose at `look_ahead_dist` along this path. | +| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command. Header.frame_id is set to `path.header.frame_id` when available and stamp to the controller node clock. | ### Topics / Services -The controller itself does not create ROS publishers/subscribers or service servers. It interacts via the `NavState` abstraction; how `NavState` is exposed (topics or other IPC mechanisms) depends on the integrating node. +The controller itself does not create ROS publishers/subscribers or service servers. It interacts via the `NavState` abstraction; how `NavState` is exposed (topics or other IPC mechanisms) depends on the integrating node. ## TF Frames + This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). TF is not directly used in this plugin. ## License -GPL-3.0-only \ No newline at end of file + +Apache-2.0 diff --git a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp index e4cb072b..0444c69a 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -1,17 +1,24 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #include -#include #include -#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" @@ -33,8 +40,8 @@ class SimpleController : public ControllerMethodBase ~SimpleController() override; /// \brief Initializes parameters and PID controllers. - /// \return std::expected success or error message. - std::expected on_initialize() override; + /// \throws std::runtime_error on initialization error. + void on_initialize() override; /// \brief Updates the controller using the given NavState. /// \param nav_state Current navigation state, including odometry and planned path. diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 1ed295db..16cda26a 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -2,16 +2,19 @@ easynav_simple_controller - 0.0.2 + 0.4.0 Easy Navigation: Simple Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake easynav_common easynav_core + rclcpp pluginlib + tf2 + tf2_geometry_msgs tf2_ros geometry_msgs nav_msgs diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp index 95c8811a..d4cd0320 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -1,27 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the SimpleController class. -#include #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "easynav_simple_controller/SimpleController.hpp" @@ -37,7 +33,7 @@ SimpleController::SimpleController() SimpleController::~SimpleController() = default; -std::expected +void SimpleController::on_initialize() { auto node = get_node(); @@ -50,8 +46,9 @@ SimpleController::on_initialize() node->declare_parameter(plugin_name + ".look_ahead_dist", look_ahead_dist_); node->declare_parameter(plugin_name + ".tolerance_dist", tolerance_dist_); node->declare_parameter(plugin_name + ".k_rot", k_rot_); - node->declare_parameter(plugin_name + ".final_goal_angle_tolerance", - final_goal_angle_tolerance_); + node->declare_parameter( + plugin_name + ".final_goal_angle_tolerance", + final_goal_angle_tolerance_); node->declare_parameter(plugin_name + ".linear_kp", linear_kp_); node->declare_parameter(plugin_name + ".linear_ki", linear_ki_); node->declare_parameter(plugin_name + ".linear_kd", linear_kd_); @@ -66,8 +63,9 @@ SimpleController::on_initialize() node->get_parameter(plugin_name + ".look_ahead_dist", look_ahead_dist_); node->get_parameter(plugin_name + ".tolerance_dist", tolerance_dist_); node->get_parameter(plugin_name + ".k_rot", k_rot_); - node->get_parameter(plugin_name + ".final_goal_angle_tolerance", - final_goal_angle_tolerance_); + node->get_parameter( + plugin_name + ".final_goal_angle_tolerance", + final_goal_angle_tolerance_); node->get_parameter(plugin_name + ".linear_kp", linear_kp_); node->get_parameter(plugin_name + ".linear_ki", linear_ki_); node->get_parameter(plugin_name + ".linear_kd", linear_kd_); @@ -85,8 +83,6 @@ SimpleController::on_initialize() last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); - - return {}; } @@ -99,7 +95,7 @@ SimpleController::update_rt(NavState & nav_state) if (!nav_state.has("path")) {return;} if (!nav_state.has("robot_pose")) {return;} - const auto path = nav_state.get("path"); + const auto & path = nav_state.get("path"); if (path.poses.empty()) { twist_stamped_.header.frame_id = path.header.frame_id; @@ -114,8 +110,20 @@ SimpleController::update_rt(NavState & nav_state) } // If we're very close to the final path pose, stop the robot. - const auto pose = nav_state.get("robot_pose").pose.pose; + const auto & pose = nav_state.get("robot_pose").pose.pose; const auto & goal_pose = path.poses.back().pose; + + const auto clock_type = get_node()->get_clock()->get_clock_type(); + rclcpp::Time latest_stamp( + nav_state.get("robot_pose").header.stamp, + clock_type); + if (rclcpp::Time( + path.poses.back().header.stamp, + latest_stamp.get_clock_type()) > latest_stamp) + { + latest_stamp = rclcpp::Time(path.poses.back().header.stamp, latest_stamp.get_clock_type()); + } + double dist_to_goal = get_distance(pose, goal_pose); double angle_to_goal = get_diff_angle(pose.orientation, goal_pose.orientation); @@ -124,16 +132,17 @@ SimpleController::update_rt(NavState & nav_state) last_vlin_ = 0.0; last_vrot_ = 0.0; twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = latest_stamp; twist_stamped_.twist.linear.x = 0.0; twist_stamped_.twist.angular.z = 0.0; // reset PID internal state to avoid windup / residual derivative if (linear_pid_) {linear_pid_->reset();} if (angular_pid_) {angular_pid_->reset();} nav_state.set("cmd_vel", twist_stamped_); - RCLCPP_DEBUG(get_node()->get_logger(), - "%s: final goal reached (dist=%.3f, ang=%.3f), stopping.", - get_plugin_name().c_str(), dist_to_goal, angle_to_goal); + RCLCPP_DEBUG( + get_node()->get_logger(), + "%s: final goal reached (dist=%.3f, ang=%.3f), stopping.", + get_plugin_name().c_str(), dist_to_goal, angle_to_goal); return; } @@ -142,8 +151,8 @@ SimpleController::update_rt(NavState & nav_state) double angle = get_angle(pose.position, ref_pose.position) - tf2::getYaw(pose.orientation); - while(angle > M_PI) {angle -= 2.0 * M_PI;} - while(angle < -M_PI) {angle += 2.0 * M_PI;} + while (angle > M_PI) {angle -= 2.0 * M_PI;} + while (angle < -M_PI) {angle += 2.0 * M_PI;} double vlin = 0.0; double vrot = 0.0; @@ -170,7 +179,7 @@ SimpleController::update_rt(NavState & nav_state) last_vrot_ = vrot; twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = latest_stamp; twist_stamped_.twist.linear.x = vlin; twist_stamped_.twist.angular.z = vrot; diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index af52d97e..c7cade9a 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include @@ -135,7 +131,8 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) EXPECT_EQ(static_map->at(1, 1), 0); } -class FriendAMCLLocalizer : public easynav::SimpleController { +class FriendAMCLLocalizer : public easynav::SimpleController +{ public: void force_path(const std::string & path) {map_path_ = path;} }; @@ -144,6 +141,13 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) { auto node = std::make_shared("test_savemap_node"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test_savemap"); auto static_map = std::make_shared(); diff --git a/controllers/easynav_vff_controller/CHANGELOG.rst b/controllers/easynav_vff_controller/CHANGELOG.rst index 6f683634..5c00f0ce 100644 --- a/controllers/easynav_vff_controller/CHANGELOG.rst +++ b/controllers/easynav_vff_controller/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package easynav_vff_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Sync to current EasyNavigation +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt index 131de417..fcb01550 100644 --- a/controllers/easynav_vff_controller/CMakeLists.txt +++ b/controllers/easynav_vff_controller/CMakeLists.txt @@ -5,17 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_common REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(nav_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io) +find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(visualization_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -25,8 +23,11 @@ set(dependencies rclcpp_lifecycle easynav_core easynav_common + easynav_sensors pluginlib visualization_msgs + geometry_msgs + nav_msgs tf2 ) @@ -41,8 +42,10 @@ target_include_directories(vff_controller PUBLIC target_link_libraries(vff_controller easynav_core::easynav_core easynav_common::easynav_common + easynav_sensors::easynav_sensors pluginlib::pluginlib ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} ${visualization_msgs_TARGETS} ${PCL_LIBRARIES} ) diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index ab2314ce..4ff60f52 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -1,28 +1,62 @@ # easynav_vff_controller -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description -Easy Navigation: VFF Controller package. + +Vector Field Histogram (VFF) style local obstacle avoidance controller. Generates `cmd_vel` commands from proximity/cost data and a target path reference using a histogram-based steering selection. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Jose Miguel Guerrero Hernandez ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_vff_controller/VffController` **Type:** `easynav::VffController` - **Base:** `easynav::ControllerMethodBase` - **Library:** `vff_controller` - **Description:** A VFF implementation for the Controller Method. + **Base Class:** `easynav::ControllerMethodBase` + **Library:** `easynav_vff_controller` + **Description:** Histogram-based obstacle avoidance controller producing velocity commands. + +## Parameters + +No ROS parameters are currently declared in code. (Add declarations in the plugin to enable runtime tuning.) + +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. +## Interfaces +### NavState Keys + +| Key | Type | Access | Notes | +|---|---|---|---| +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current pose for steering decisions. | +| `path` | `nav_msgs::msg::Path` | **Read** | Planned path reference. | +| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command. | + +### Publications + +| Topic | Type | Purpose | QoS | +|---|---|---|---| +| `/vff/markers` | `visualization_msgs/msg/MarkerArray` | Histogram / debug visualization (name inferred from code marker publisher variable). | depth=10 | + +### Subscriptions / Services + +None directly; relies on NavState for data sharing. + +## TF Frames + +Relies on frames stamped in `robot_pose`; does not query TF directly. ## License -GPL-3.0-only + +Apache-2.0 diff --git a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp index 3c206053..66dd24e4 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the VffController method. @@ -23,11 +19,9 @@ #ifndef EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ #define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ -#include #include "pcl/point_cloud.h" #include "easynav_core/ControllerMethodBase.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" @@ -83,11 +77,9 @@ class VffController : public easynav::ControllerMethodBase * This method is called once during the configuration phase of the controller node, * and can be optionally overridden by derived classes to perform custom setup logic. * - * @return std::expected Returns an expected object: - * - `void` if initialization was successful, - * - a `std::string` containing an error message if initialization failed. + * @throws std::runtime_error on initialization error. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the localization estimate based on the current navigation state. diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index bf826f5e..f4e84caa 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -2,10 +2,10 @@ easynav_vff_controller - 0.0.2 + 0.4.0 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake @@ -15,7 +15,9 @@ easynav_common nav_msgs easynav_core + easynav_sensors pcl_ros + geometry_msgs visualization_msgs ament_lint_auto diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 9557a177..2ce95429 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -1,33 +1,27 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the VffController class. -#include #include "easynav_vff_controller/VffController.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/goals.hpp" -#include "nav_msgs/msg/path.hpp" #include #include @@ -35,7 +29,7 @@ namespace easynav { -std::expected VffController::on_initialize() +void VffController::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -51,8 +45,9 @@ std::expected VffController::on_initialize() node->declare_parameter(plugin_name + ".max_speed", 0.8); node->declare_parameter(plugin_name + ".max_angular_speed", 1.5); - node->get_parameter(plugin_name + ".distance_obstacle_detection", - distance_obstacle_detection_); + node->get_parameter( + plugin_name + ".distance_obstacle_detection", + distance_obstacle_detection_); node->get_parameter(plugin_name + ".obstacle_detection_x_min", obstacle_detection_x_min_); node->get_parameter(plugin_name + ".obstacle_detection_x_max", obstacle_detection_x_max_); node->get_parameter(plugin_name + ".obstacle_detection_y_min", obstacle_detection_y_min_); @@ -62,9 +57,11 @@ std::expected VffController::on_initialize() node->get_parameter(plugin_name + ".max_speed", max_speed_); node->get_parameter(plugin_name + ".max_angular_speed", max_angular_speed_); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + // Initialize the odometry message cmd_vel_.header.stamp = node->now(); - cmd_vel_.header.frame_id = get_tf_prefix() + "base_link"; + cmd_vel_.header.frame_id = tf_info.robot_frame; cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.linear.y = 0.0; cmd_vel_.twist.linear.z = 0.0; @@ -75,8 +72,6 @@ std::expected VffController::on_initialize() // Publisher for visualization markers marker_array_pub_ = node->create_publisher( "vff/markers_vff", 10); - - return {}; } visualization_msgs::msg::MarkerArray @@ -213,9 +208,10 @@ void VffController::update_rt(NavState & nav_state) if (!nav_state.has("robot_pose")) {return;} const auto & all_goals = nav_state.get("goals"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (all_goals.goals.empty()) { - cmd_vel_.header.frame_id = get_tf_prefix() + "map"; + cmd_vel_.header.frame_id = tf_info.map_frame; cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.angular.z = 0.0; @@ -257,18 +253,21 @@ void VffController::update_rt(NavState & nav_state) // Calculate the angle error double angle_error = normalize_angle(bearing - yaw); - const auto perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_no_group(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) - .fuse(get_tf_prefix() + "base_link") - ->filter({obstacle_detection_x_min_, obstacle_detection_y_min_, obstacle_detection_z_min_}, - {obstacle_detection_x_max_, obstacle_detection_y_max_, - obstacle_detection_z_max_}).as_points(); + .fuse(tf_info.robot_frame) + .filter( + {obstacle_detection_x_min_, obstacle_detection_y_min_, obstacle_detection_z_min_}, + {obstacle_detection_x_max_, obstacle_detection_y_max_, + obstacle_detection_z_max_}) + .as_points(); // Get VFF vectors - const VFFVectors & vff = get_vff(angle_error, fused, get_tf_prefix() + "base_link"); + const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_frame); // Use result vector to calculate output speed const auto & v = vff.result; @@ -279,8 +278,9 @@ void VffController::update_rt(NavState & nav_state) cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = std::clamp(module, 0.0, max_speed_); cmd_vel_.twist.angular.z = std::clamp(angle, -max_angular_speed_, max_angular_speed_); - RCLCPP_INFO(get_node()->get_logger(), "[distance: %.2f, yaw_error: %.2f]", distance, - angle_error); + RCLCPP_INFO( + get_node()->get_logger(), "[distance: %.2f, yaw_error: %.2f]", distance, + angle_error); nav_state.set("cmd_vel", cmd_vel_); } diff --git a/localizers/easynav_costmap_localizer/CHANGELOG.rst b/localizers/easynav_costmap_localizer/CHANGELOG.rst index d0592d8f..92be0a26 100644 --- a/localizers/easynav_costmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_costmap_localizer/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package easynav_costmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Avoid creating objects every loop cycle +* Fix std::normal_distribution when stdev is zero +* Check if std is non-positive before creating std::normal_distribution + Dynamic base costmap +* Change map static to base +* Reset pose from RViz2 for every localizer +* Adaptations to `#94 `_ +* Merge branch 'EasyNavigation:rolling' into rolling +* GPLv3 -> Apache 2.0 +* Sync to current EasyNavigation +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index 1ea83e63..65265795 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -5,15 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_costmap_common REQUIRED) find_package(easynav_localizer REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -21,6 +18,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) add_library(${PROJECT_NAME} SHARED @@ -34,9 +32,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_costmap_common::easynav_costmap_common easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib + Eigen3::Eigen ${tf2_geometry_msgs_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -62,7 +62,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") @@ -77,11 +77,13 @@ ament_export_dependencies( easynav_costmap_common easynav_localizer easynav_core + easynav_sensors tf2 tf2_ros pluginlib geometry_msgs std_srvs nav_msgs + eigen ) ament_package() diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8d3ae420..998ee961 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -1,21 +1,25 @@ # easynav_costmap_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for prediction. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_costmap_localizer/AMCLLocalizer` - **Type:** `easynav::AMCLLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,6 +27,7 @@ AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for predi - **Description:** AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for prediction. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_costmap_localizer/AMCLLocalizer/...`. | Name | Type | Default | Description | @@ -41,33 +46,35 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.min_noise_yaw` | `double` | `0.05` | Minimum yaw noise (rad). | | `.compute_odom_from_tf` | `bool` | `false` | If true, read odometry from TF (odom->base_footprint) instead of /odom topic. | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false. | SensorDataQoS (reliable) | +| Subscription | `odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false. | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Init particles pose to the received pose, using covariance | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set. | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Estimated pose with covariance. | depth=10 | - ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | -| `map.static` | `Costmap2D` | **Read** | Static costmap for likelihood evaluation. | - +| `map.base` | `Costmap2D` | **Read** | Base costmap for likelihood evaluation. | ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map -> odom` | Aligns the odometry frame with the map frame. | | Requires (optional) | `odom -> base_footprint` | Required when compute_odom_from_tf=true (read from TF). | - ## License -GPL-3.0-only + +Apache-2.0 diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp index 19b81a81..4576dc13 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the AMCLLocalizer method. @@ -24,11 +20,6 @@ #define EASYNAV_COSTMAP_LOCALIZER__AMCLLOCALIZER_HPP_ #include -#include -#include -#include -#include -#include #include #include @@ -72,9 +63,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error on initialization error. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -164,6 +155,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -171,6 +165,14 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + + /** * @brief Update odom from TFs instead of a odom topic * @@ -181,7 +183,7 @@ class AMCLLocalizer : public LocalizerMethodBase std::vector particles_; /// Random number generator used for sampling noise. - std::default_random_engine rng_; + std::mt19937 rng_; /// Current estimated odometry-based pose. nav_msgs::msg::Odometry pose_; @@ -216,6 +218,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Time interval (in seconds) after which the particles should be reseeded. double reseed_time_; + /// Timestamp of the last input message (odometry or initial pose). + rclcpp::Time last_input_time_; + /// Timestamp of the last reseed event. rclcpp::Time last_reseed_; }; diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index fbd2abd3..e97e0574 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,10 +2,10 @@ easynav_costmap_localizer - 0.0.2 + 0.4.0 Easy Navigation: Costmap Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake @@ -13,12 +13,14 @@ easynav_costmap_common easynav_localizer easynav_core + easynav_sensors pluginlib tf2 tf2_ros tf2_geometry_msgs geometry_msgs nav_msgs + eigen rclcpp rclcpp_lifecycle diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 912b94a2..65a353b8 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -1,34 +1,28 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the AMCLLocalizer class using Costmap2D. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -162,6 +156,7 @@ using namespace std::chrono_literals; AMCLLocalizer::AMCLLocalizer() +: rng_(std::random_device{}()) { NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { @@ -178,7 +173,8 @@ AMCLLocalizer::AMCLLocalizer() double roll, pitch, yaw; tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; + ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << " } Odometry with pose: (x: " << + x << ", y: " << y << ", yaw: " << yaw << ")"; return ret.str(); }); } @@ -187,7 +183,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -218,7 +214,8 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); node->get_parameter(plugin_name + ".noise_translation", noise_translation_); node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", + node->get_parameter( + plugin_name + ".noise_translation_to_rotation", noise_translation_to_rotation_); node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); @@ -228,13 +225,28 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); reseed_time_ = 1.0 / reseed_freq; + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", + RCLCPP_INFO( + node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -262,15 +274,18 @@ AMCLLocalizer::on_initialize() } particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", + 10); estimate_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); + + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); last_reseed_ = get_node()->now(); + last_input_time_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - - return {}; } void printTransform(const tf2::Transform & tf) @@ -319,6 +334,7 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) if (compute_odom_from_tf_) {return;} tf2::fromMsg(msg->pose.pose, odom_); + last_input_time_ = msg->header.stamp; if (!initialized_odom_) { last_odom_ = odom_; @@ -326,13 +342,169 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) } } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + // Check expected frame + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; // C(0,0) + double cov_xy = cov[1]; // C(0,1) + double var_y = cov[7]; // C(1,1) + + // Yaw variance + double var_yaw = cov[35]; // C(5,5) + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = + std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); + + const std::size_t N = particles_.size(); + + for (std::size_t i = 0; i < N; ++i) { + // Sample a correlated 2D perturbation according to covariance(x,y) + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + + // Sample yaw + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights as safety measure + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + + void AMCLLocalizer::update_odom_from_tf() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); + tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); return; @@ -343,8 +515,11 @@ AMCLLocalizer::update_odom_from_tf() last_odom_ = odom_; odom_ = tf_odom; + last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; + + } void @@ -372,24 +547,24 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) tf2::Matrix3x3(delta.getRotation()).getRPY(roll, pitch, yaw); double rot_len = std::abs(yaw); - std::random_device rd; - std::mt19937 gen(rd()); - - for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution noise_yaw( - 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( - dx + noise_dx(gen), - dy + noise_dy(gen), - dz + noise_dz(gen)); + dx + noise_dx(rng_), + dy + noise_dy(rng_), + dz + noise_dz(rng_)); - double noisy_yaw = yaw + noise_yaw(gen); + double noisy_yaw = yaw + noise_yaw(rng_); tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_yaw); @@ -410,27 +585,28 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto perceptions = nav_state.get("points"); - - if (!nav_state.has("map.static")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); + if (!nav_state.has("map.base")) { + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map"); return; } - const auto & map_static = nav_state.get("map.static"); + const auto & map_static = nav_state.get("map.base"); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - const auto & filtered = PointPerceptionsOpsView(perceptions) - .downsample(map_static.getResolution()) - .fuse(get_tf_prefix() + "base_footprint") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .collapse({NAN, NAN, 0.1}) - ->downsample(map_static.getResolution()) - .as_points(); + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(map_static.getResolution()) + .fuse(tf_info.robot_footprint_frame, last_input_time_) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .collapse({NAN, NAN, 0.1}) + .downsample(map_static.getResolution()); + const auto & filtered = view.as_points(); if (filtered.empty()) { RCLCPP_WARN(get_node()->get_logger(), "No points to correct"); @@ -490,7 +666,8 @@ AMCLLocalizer::reseed() const std::size_t N = particles_.size(); const std::size_t N_top = N / 2; - std::sort(particles_.begin(), particles_.end(), + std::sort( + particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -509,7 +686,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { @@ -527,8 +704,9 @@ AMCLLocalizer::reseed() double dx = l00 * z0; double dy = l10 * z0 + l11 * z1; - std::normal_distribution xy_noise(0.0, std::max(sqrt(dx * dx + dy * dy), - min_noise_xy_)); + std::normal_distribution xy_noise(0.0, std::max( + sqrt(dx * dx + dy * dy), + min_noise_xy_)); tf2::Vector3 new_origin(origin.x() + xy_noise(rng_), origin.y() + xy_noise(rng_), 0.0); @@ -562,9 +740,10 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; + tf_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + tf_msg.header.frame_id = tf_info.map_frame; + tf_msg.child_frame_id = tf_info.odom_frame; tf_msg.transform = tf2::toMsg(map2bf); RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); @@ -574,10 +753,11 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf) void AMCLLocalizer::publishParticles() { - geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseArray array_msg; + array_msg.header.stamp = last_input_time_; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { geometry_msgs::msg::Pose pose_msg; @@ -605,7 +785,8 @@ AMCLLocalizer::getEstimatedPose() const const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -635,9 +816,10 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); double yaw_variance = computeYawVariance(particles_, 0, N_top); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.stamp = last_input_time_; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -659,9 +841,10 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; + odom_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_msg.header.frame_id = tf_info.map_frame; + odom_msg.child_frame_id = tf_info.robot_frame; tf2::Transform est_pose = getEstimatedPose(); @@ -675,7 +858,8 @@ AMCLLocalizer::get_pose() const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 9b2fe4fc..266b62a6 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,161 +1,184 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include +#include +#include +#include +#include +#include + #include "easynav_costmap_localizer/AMCLLocalizer.hpp" -#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_localizer/LocalizerNode.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto pub = node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - auto static_map = std::make_shared(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); +} // namespace - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.25; + const double y0 = -2.5; + const double yaw0 = 0.4; - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); + const double x1 = -0.75; + const double y1 = 0.9; + const double yaw1 = -1.2; - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared(options); + auto localizer = std::make_shared(); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + localizer->initialize(node, "test"); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + ASSERT_NE(localizer->init_pose_sub_, nullptr); - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); + } - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); + } - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/localizers/easynav_fusion_localizer/CHANGELOG.rst b/localizers/easynav_fusion_localizer/CHANGELOG.rst new file mode 100644 index 00000000..a00bb215 --- /dev/null +++ b/localizers/easynav_fusion_localizer/CHANGELOG.rst @@ -0,0 +1,31 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_fusion_localizer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* deleted tests related to initial pose param which was removed. Removed spaces +* fix: add gnss group to gps config and ge_group has group to meet new easynav features. Initial pose now changes initial utm pose to change robot position in the map +* Update README.md for all localizers +* Reset pose from RViz2 for every localizer +* Adaptations to `#94 `_ +* Update plugins to new sensors API + New version of fusion localizer tested in real robot and simulator +* Added config file for gazebo simulation demo +* Only starts filter if params are found +* params updated +* Added simple configuration to launch full demo +* Initialization considers IMU orientation +* Merge branch 'rolling' of github.com:midemig/easynav_plugins into rolling +* Call set pose srvc for first pose for fastest convergence. Updated params to stabilice the filter in summit +* GPLv3 -> Apache 2.0 +* fixed odom topic +* Several bugs fixed. Now it correctly integrates GPS measurements to the dual filter. +* Added dual ukf for local and global localization +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, midemig, migueldm + +0.0.2 (2025-11-23) +------------------ +* First version +* Contributors: Miguel Ángel de Miguel diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt new file mode 100644 index 00000000..a2841325 --- /dev/null +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -0,0 +1,129 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_fusion_localizer) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_localizer REQUIRED) +find_package(easynav_sensors REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(robot_localization REQUIRED) +find_package(std_srvs REQUIRED) +find_package(angles REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) + +# Geographiclib installs FindGeographicLib.cmake to this non-standard location +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) + +include_directories(${GeographicLib_INCLUDE_DIRS}) + + +set(dependencies + rclcpp + rclcpp_lifecycle + easynav_core + easynav_common + easynav_localizer + easynav_sensors + pluginlib + geographic_msgs + robot_localization + diagnostic_msgs + diagnostic_updater + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + eigen +) + +add_library(fusion_localizer SHARED + src/easynav_fusion_localizer/FusionLocalizer.cpp + src/easynav_fusion_localizer/ukf_wrapper.cpp +) +target_include_directories(fusion_localizer PUBLIC + $ + $ + ${robot_localization_INCLUDE_DIRS} + ${std_srvs_INCLUDE_DIRS} +) +target_link_libraries(fusion_localizer + easynav_core::easynav_core + easynav_common::easynav_common + easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors + pluginlib::pluginlib + ${geographic_msgs_TARGETS} + GeographicLib + ${GeographicLib_LIBRARIES} + ${robot_localization_LIBRARIES} + angles::angles + yaets::yaets + diagnostic_updater::diagnostic_updater + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + Eigen3::Eigen +) + +target_compile_definitions(fusion_localizer PUBLIC EASYNAV_DEBUG_WITH_YAETS) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(TARGETS + fusion_localizer + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories(include) +ament_export_libraries( + fusion_localizer +) +ament_export_targets( + export_${PROJECT_NAME} +) +# Register the localization plugins +pluginlib_export_plugin_description_file(easynav_core easynav_fusion_localizer_plugins.xml) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/localizers/easynav_fusion_localizer/COLCON_IGNORE b/localizers/easynav_fusion_localizer/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md new file mode 100644 index 00000000..c641ef72 --- /dev/null +++ b/localizers/easynav_fusion_localizer/README.md @@ -0,0 +1,62 @@ +# easynav_fusion_localizer + +[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) + +## Description +Odometry fusion localizer based on Robot Localization that fuses any n odometry sources. + +## Authors and Maintainers +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Miguel Ángel de Miguel + +## Supported ROS 2 Distributions +| Distribution | Status | +|---|---| +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) +- **Plugin Name:** `easynav_fusion_localizer/FusionLocalizer` +- **Type:** `easynav::FusionLocalizer` +- **Base Class:** `easynav::LocalizerMethodBase` +- **Library:** `fusion_localizer` +- **Description:** Odometry fusion localizer based on Robot Localization that fuses any n odometry sources. + +## Parameters + +All parameters are declared under the plugin namespace, i.e., `//easynav_fusion_localizer/FusionLocalizer/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | +| `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | +| `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | + +For additional robot_localization-related configuration, see the upstream documentation: +https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html + +## Interfaces (Topics and Services) + +### Subscriptions and Publications +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the filter pose from an external initial pose estimate. | depth=10 | +| Publisher | `odometry/filtered` | `nav_msgs/msg/Odometry` | Odometry fused from all sources. | SensorDataQoS | + + +### Services +This package does not create service servers or clients. + +## NavState Keys +| Key | Type | Access | Notes | +|---|---|---|---| +| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Fused odometry estimate. | + + +## TF Frames +| Role | Transform | Notes | +|---|---|---| +| Publishes | `map -> odom` | Transform that aligns the odometry frame with the global frame. | + + +## License +Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml new file mode 100644 index 00000000..905be4e5 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params.yaml @@ -0,0 +1,133 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [dummy] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 40.2834931 + longitude_origin: -3.8207253999999997 + altitude_origin: 746.184 + global_filter: + two_d_mode: true + publish_tf: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS Odometry --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + true, true, false, + false, false, false, + true, true, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + twist0: /gps/fix_velocity + twist0_config: + [false, false, false, + false, false, false, + true, false, false, + false, false, false, + false, false, false] + twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [dummy] + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [dummy] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [imu, gps] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.1 + angle_tolerance: 0.05 diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml new file mode 100644 index 00000000..40b6d005 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -0,0 +1,280 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 40.2834931 + longitude_origin: -3.8207253999999997 + altitude_origin: 746.184 + local_filter: + two_d_mode: true + publish_tf: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # imu0_differential: false + # imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, false, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS Odometry --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + # --- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, true, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/square.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [imu, gps] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml new file mode 100644 index 00000000..7ec21016 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -0,0 +1,281 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 40.284796759605115 + longitude_origin: -3.8214099132625017 + altitude_origin: 669.9730248888955 + local_filter: + two_d_mode: true + publish_tf: false + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # imu0_differential: false + # imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, false, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS Odometry --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + # --- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, true, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/empty.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [imu, gps] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + group: gnss + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 diff --git a/localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml b/localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml new file mode 100644 index 00000000..cf052497 --- /dev/null +++ b/localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml @@ -0,0 +1,9 @@ + + + + + A Kalman Filter implementation for the Localizer Method. + + + + diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp new file mode 100644 index 00000000..740a03df --- /dev/null +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include +#include + +#include "easynav_core/LocalizerMethodBase.hpp" +#include "easynav_fusion_localizer/ukf_wrapper.hpp" + +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +namespace easynav +{ + +constexpr double kNoOrientationCovariance = 1e5; + +/** + * @class FusionLocalizer + * @brief Plugin de localización para EasyNav que integra el UKF de + * robot_localization para la fusión de sensores. + */ +class FusionLocalizer : public easynav::LocalizerMethodBase +{ +public: + FusionLocalizer() = default; + virtual ~FusionLocalizer() = default; + +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + +protected: + /** + * @brief Hook de inicialización de MethodBase. + * + * Loads and initializes the UkfWrapper, which will load parameters + * and create all robot_localization subscribers/publishers. + * + * @throws std::runtime_error if initialization fails. + */ + void on_initialize() override; + + /** + * @brief Hook de actualización RT (alta frecuencia) de LocalizerMethodBase. + * + * Esta función actúa como nuestro "timer" manual. Llama al ciclo + * principal (periodicUpdate) del filtro UKF y actualiza el NavState + * de easynav con la pose filtrada. + */ + void update_rt(NavState & nav_state) override; + + /** + * @brief Hook de actualización no-RT (baja frecuencia). + * + * En esta implementación, toda la lógica principal reside en update_rt. + */ + void update(NavState & nav_state) override; + +private: + std::unique_ptr ukf_local_{nullptr}; + std::unique_ptr ukf_global_{nullptr}; + + bool has_global_filter_{false}; + bool has_local_filter_{false}; + + int n_gps_sensors_{0}; + + geometry_msgs::msg::PoseWithCovarianceStamped + navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); + + sensor_msgs::msg::NavSatFix + odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg); + + double latitude_origin_{0.0}; + double longitude_origin_{0.0}; + double altitude_origin_{0.0}; + double UTM_origin_x_{0.0}; + double UTM_origin_y_{0.0}; + double UTM_origin_z_{0.0}; + std::string UTM_zone_; + int UTM_zone_number_{0}; + bool UTM_zone_northp_{true}; + + std::vector last_gps_stamp_; + + rclcpp::Publisher::SharedPtr navsat_pub_{nullptr}; + std::string navsatfix_topic_; + + bool first_pose_received_{false}; + +}; + +} // namespace easynav diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp new file mode 100644 index 00000000..1c7f4d35 --- /dev/null +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -0,0 +1,875 @@ +/* + * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef EASYNAV_FUSION_LOCALIZER__UKF_WRAPPER_HPP_ +#define EASYNAV_FUSION_LOCALIZER__UKF_WRAPPER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "Eigen/Dense" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "robot_localization/filter_state.hpp" +#include "robot_localization/measurement.hpp" +#include "robot_localization/srv/toggle_filter_processing.hpp" +#include "robot_localization/srv/set_pose.hpp" +#include "robot_localization/ukf.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include +#include + +namespace robot_localization +{ + +struct CallbackData +{ + CallbackData( + const std::string & topic_name, + const std::vector & update_vector, const int update_sum, + const bool differential, const bool relative, + const bool pose_use_child_frame, + const double rejection_threshold) + : topic_name_(topic_name), update_vector_(update_vector), + update_sum_(update_sum), differential_(differential), + relative_(relative), pose_use_child_frame_(pose_use_child_frame), + rejection_threshold_(rejection_threshold) {} + + std::string topic_name_; + std::vector update_vector_; + int update_sum_; + bool differential_; + bool relative_; + bool pose_use_child_frame_; + double rejection_threshold_; +}; + +using MeasurementQueue = + std::priority_queue, + Measurement>; +using MeasurementHistoryDeque = std::deque; +using FilterStateHistoryDeque = std::deque; + +class UkfWrapper +{ +public: + //! @brief Constructor + //! + //! The UkfWrapper constructor makes sure that anyone using + //! this template is doing so with the correct object type + //! + explicit UkfWrapper( + std::shared_ptr parent_node, + const std::string & tf_prefix, + const std::string & plugin_name, + bool local_filter); + + //! @brief Destructor + //! + //! Clears out the message filters and topic subscribers. + //! + ~UkfWrapper(); + + //! @brief Resets the filter to its initial state + //! + void reset(); + + //! @brief Service callback to toggle processing measurements for a standby + //! mode but continuing to publish + //! @param[in] request - The state requested, on (True) or off (False) + //! @param[out] response - status if upon success + //! @return boolean true if successful, false if not + //! + void toggleFilterProcessingCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Request> req, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Response> resp); + + //! @brief Callback method for receiving all acceleration (IMU) messages + //! @param[in] msg - The ROS IMU message to take in. + //! @param[in] callback_data - Relevant static callback data + //! @param[in] target_frame - The target frame_id into which to transform the + //! data + //! + void accelerationCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const CallbackData & callback_data, + const std::string & target_frame); + + //! @brief Callback method for receiving non-stamped control input + //! @param[in] msg - The ROS twist message to take in + //! + void controlCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + //! @brief Callback method for receiving stamped control input + //! @param[in] msg - The ROS stamped twist message to take in + //! + void + controlStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); + + //! @brief Differentiate angular velocity for angular acceleration + //! + //! @param[in] currentTime - The time at which to carry out differentiation (the current time) + //! + //! Maybe more state variables can be time-differentiated to estimate higher-order states, + //! but now we only focus on obtaining the angular acceleration. It implements a backward- + //! Euler differentiation. + //! + void differentiateMeasurements(const rclcpp::Time & current_time); + + //! @brief Adds a measurement to the queue of measurements to be processed + //! + //! @param[in] topic_name - The name of the measurement source (only used for + //! debugging) + //! @param[in] measurement - The measurement to enqueue + //! @param[in] measurement_covariance - The covariance of the measurement + //! @param[in] update_vector - The boolean vector that specifies which + //! variables to update from this measurement + //! @param[in] mahalanobis_thresh - Threshold, expressed as a Mahalanobis + //! distance, for outlier rejection + //! @param[in] time - The time of arrival (in seconds) + //! + void enqueueMeasurement( + const std::string & topic_name, + const Eigen::VectorXd & measurement, + const Eigen::MatrixXd & measurement_covariance, + const std::vector & update_vector, + const double mahalanobis_thresh, + const rclcpp::Time & time); + + //! @brief Method for zeroing out 3D variables within measurements + //! @param[out] measurement - The measurement whose 3D variables will be + //! zeroed out + //! @param[out] measurement_covariance - The covariance of the measurement + //! @param[out] updateVector - The boolean update vector of the measurement + //! + //! If we're in 2D mode, then for every measurement from every sensor, we call + //! this. It sets the 3D variables to 0, gives those variables tiny variances, + //! and sets their updateVector values to 1. + //! + void forceTwoD( + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance, + std::vector & update_vector); + + //! @brief Method to get filter + //! @param[out] filter - the underlying templated filter + //! + robot_localization::Ukf & getFilter() + { + return filter_; + } + + //! @brief Retrieves the EKF's output for broadcasting + //! @param[out] message - The standard ROS odometry message to be filled + //! @return true if the filter is initialized, false otherwise + //! + bool getFilteredOdometryMessage(nav_msgs::msg::Odometry * message); + + //! @brief Retrieves the EKF's acceleration output for broadcasting + //! @param[out] message - The standard ROS acceleration message to be filled + //! @return true if the filter is initialized, false otherwise + //! + bool getFilteredAccelMessage( + geometry_msgs::msg::AccelWithCovarianceStamped * message); + + //! @brief Callback method for receiving all IMU messages + //! @param[in] msg - The ROS IMU message to take in. + //! @param[in] topic_name - The topic name for the IMU message (only used for + //! debug output) + //! @param[in] pose_callback_data - Relevant static callback data for + //! orientation variables + //! @param[in] twist_callback_data - Relevant static callback data for angular + //! velocity variables + //! @param[in] accel_callback_data - Relevant static callback data for linear + //! acceleration variables + //! + //! This method separates out the orientation, angular velocity, and linear + //! acceleration data and passed each on to its respective callback. + //! + void imuCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data, + const CallbackData & accel_callback_data); + + //! @brief Processes all measurements in the measurement queue, in temporal + //! order + //! + //! @param[in] current_time - The time at which to carry out integration (the + //! current time) + //! + void integrateMeasurements(const rclcpp::Time & current_time); + + //! @brief Loads all parameters from file + //! + void loadParams(); + + //! @brief callback function which is called for periodic updates + //! + void periodicUpdate(); + + //! @brief Callback method for receiving all odometry messages + //! @param[in] msg - The ROS odometry message to take in. + //! @param[in] topic_name - The topic name for the odometry message (only used + //! for debug output) + //! @param[in] pose_callback_data - Relevant static callback data for pose + //! variables + //! @param[in] twist_callback_data - Relevant static callback data for twist + //! variables + //! + //! This method simply separates out the pose and twist data into two new + //! messages, and passes them into their respective callbacks + //! + void odometryCallback( + const nav_msgs::msg::Odometry::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data); + + //! @brief Callback method for receiving all pose messages + //! @param[in] msg - The ROS stamped pose with covariance message to take in + //! @param[in] callback_data - Relevant static callback data + //! @param[in] target_frame - The target frame_id into which to transform the + //! data + //! @param[in] pose_source_frame - The source frame_id from which to transform + //! the data + //! @param[in] imu_data - Whether this data comes from an IMU + //! + void poseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame, + const std::string & pose_source_frame, + const bool imu_data); + + //! @brief initialize the filter + //! + void initialize(); + + //! @brief Service callback for resetting the filter to its initial state. Parameters are unused. + //! + void resetSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr); + + //! @brief Callback method for manually setting/resetting the internal pose + //! estimate + //! @param[in] msg - The ROS stamped pose with covariance message to take in + //! + void setPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + //! @brief Service callback for manually setting/resetting the internal pose + //! estimate + //! + //! @param[in] request - Custom service request with pose information + //! @return true if successful, false if not + bool setPoseSrvCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Service callback for manually enable the filter + //! @param[in] request - N/A + //! @param[out] response - N/A + //! @return boolean true if successful, false if not + bool enableFilterSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr); + + //! @brief Callback method for receiving all twist messages + //! @param[in] msg - The ROS stamped twist with covariance message to take in. + //! @param[in] callback_data - Relevant static callback data + //! @param[in] target_frame - The target frame_id into which to transform the + //! data + //! + void twistCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame); + + //! @brief Validates filter outputs for NaNs and Infinite values + //! @param[out] message - The standard ROS odometry message to be validated + //! @return true if the filter output is valid, false otherwise + //! + bool validateFilterOutput(nav_msgs::msg::Odometry * message); + + const std::vector & getGpsCallbackDataArr() const + { + return gps_callbackData_arr_; + } + +protected: + //! @brief Finds the latest filter state before the given timestamp and makes + //! it the current state again. + //! + //! This method also inserts all measurements between the older filter + //! timestamp and now into the measurements queue. + //! + //! @param[in] time - The time to which the filter state should revert + //! @return True if restoring the filter succeeded. False if not. + //! + bool revertTo(const rclcpp::Time & time); + + //! @brief Saves the current filter state in the queue of previous filter + //! states + //! + //! These measurements will be used in backwards smoothing in the event that + //! older measurements come in. + //! @param[in] filter - The filter base object whose state we want to save + //! + void saveFilterState(robot_localization::Ukf & filter); + + //! @brief Removes measurements and filter states older than the given cutoff + //! time. + //! @param[in] cutoff_time - Measurements and states older than this time will + //! be dropped. + //! + void clearExpiredHistory(const rclcpp::Time cutoff_time); + + //! @brief Clears measurement queue + //! + void clearMeasurementQueue(); + + //! @brief Adds a diagnostic message to the accumulating map and updates the + //! error level + //! @param[in] error_level - The error level of the diagnostic + //! @param[in] topic_and_class - The sensor topic (if relevant) and class of + //! diagnostic + //! @param[in] message - Details of the diagnostic + //! @param[in] is_static - Whether or not this diagnostic information is + //! static + //! + void addDiagnostic( + const int error_level, const std::string & topic_and_class, + const std::string & message, const bool is_static); + + //! @brief Aggregates all diagnostics so they can be published + //! @param[in] wrapper - The diagnostic status wrapper to update + //! + void aggregateDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper & wrapper); + + //! @brief Utility method for copying covariances from ROS covariance arrays + //! to Eigen matrices + //! + //! This method copies the covariances and also does some data validation, + //! which is why it requires more parameters than just the covariance + //! containers. + //! @param[in] covariance_in - The source array for the covariance data + //! @param[in] covariance_out - The destination matrix for the covariance data + //! @param[in] topic_name - The name of the source data topic (for debug + //! purposes) + //! @param[in] update_vector - The update vector for the source topic + //! @param[in] offset - The "starting" location within the array/update vector + //! @param[in] dimension - The number of values to copy, starting at the + //! offset + //! + void copyCovariance( + const double * covariance_in, + Eigen::MatrixXd & covariance_out, + const std::string & topic_name, + const std::vector & update_vector, + const size_t offset, const size_t dimension); + + //! @brief Utility method for copying covariances from Eigen matrices to ROS + //! covariance arrays + //! + //! @param[in] covariance_in - The source matrix for the covariance data + //! @param[in] covariance_out - The destination array + //! @param[in] dimension - The number of values to copy + //! + void copyCovariance( + const Eigen::MatrixXd & covariance_in, + double * covariance_out, const size_t dimension); + + //! @brief Loads fusion settings from the config file + //! @param[in] topic_name - The name of the topic for which to load settings + //! @return The boolean vector of update settings for each variable for this + //! topic + //! + std::vector loadUpdateConfig(const std::string & topic_name); + + //! @brief Prepares an IMU message's linear acceleration for integration into + //! the filter + //! @param[in] msg - The IMU message to prepare + //! @param[in] topic_name - The name of the topic over which this message was + //! received + //! @param[in] target_frame - The target tf frame + //! @param[in] relative - whether the IMU sensor reports pose relative to + //! initialization pose + //! @param[in] update_vector - The update vector for the data source + //! @param[in] measurement - The twist data converted to a measurement + //! @param[in] measurement_covariance - The covariance of the converted + //! measurement + //! + bool prepareAcceleration( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const std::string & target_frame, + const bool relative, + std::vector & update_vector, + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance); + + //! @brief Prepares a pose message for integration into the filter + //! @param[in] msg - The pose message to prepare + //! @param[in] topic_name - The name of the topic over which this message was + //! received + //! @param[in] target_frame - The target tf frame + //! @param[in] source_frame - The source tf frame + //! @param[in] differential - Whether we're carrying out differential + //! integration + //! @param[in] relative - Whether this measurement is processed relative to + //! the first + //! @param[in] imuData - Whether this measurement is from an IMU + //! @param[in,out] updateVector - The update vector for the data source + //! @param[out] measurement - The pose data converted to a measurement + //! @param[out] measurementCovariance - The covariance of the converted + //! measurement + //! @return true indicates that the measurement was successfully prepared, + //! false otherwise + //! + bool preparePose( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const std::string & topic_name, const std::string & target_frame, + const std::string & source_frame, + const bool differential, const bool relative, const bool imu_data, + std::vector & update_vector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance); + + //! @brief Prepares a twist message for integration into the filter + //! @param[in] msg - The twist message to prepare + //! @param[in] topicName - The name of the topic over which this message was + //! received + //! @param[in] targetFrame - The target tf frame + //! @param[in,out] updateVector - The update vector for the data source + //! @param[out] measurement - The twist data converted to a measurement + //! @param[out] measurementCovariance - The covariance of the converted + //! measurement + //! @return true indicates that the measurement was successfully prepared, + //! false otherwise + //! + bool prepareTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const std::string & topicName, const std::string & targetFrame, + std::vector & updateVector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurementCovariance); + + std::shared_ptr parent_node_{nullptr}; + + //! @brief Whether or not we print diagnostic messages to the /diagnostics + //! topic + //! + bool print_diagnostics_; + + //! @brief Whether we publish the acceleration + //! + bool publish_acceleration_; + + //! @brief Whether we publish the transform from the world_frame to the + //! base_link_frame + //! + bool publish_transform_; + + //! @brief Whether to reset the filters when backwards jump in time is + //! detected + //! + //! This is usually the case when logs are being used and a jump in the logi + //! is done or if a log files restarts from the beginning. + //! + bool reset_on_time_jump_; + + //! @brief Whether or not we use smoothing + //! + bool smooth_lagged_data_; + + //! @brief Whether the filter should process new measurements or not. + //! + bool toggled_on_; + + //! @brief Whether or not we're in 2D mode + //! + //! If this is true, the filter binds all 3D variables (Z, + //! roll, pitch, and their respective velocities) to 0 for + //! every measurement. + //! + bool two_d_mode_; + + //! @brief Whether or not we use a control term + //! + bool use_control_; + + //! @brief Whether or not we use a stamped control term + //! + bool stamped_control_; + + //! @brief Start the Filter disabled at startup + //! + //! If this is true, the filter reads parameters and prepares publishers and subscribes + //! but does not integrate new messages into the state vector. + //! The filter can be enabled later using a service. + bool disabled_at_startup_; + + //! @brief Whether the filter is enabled or not. See disabledAtStartup_. + bool enabled_; + + //! @brief Whether we'll allow old measurements to cause a re-publication of the updated state + bool permit_corrected_publication_; + + //! @brief The max (worst) dynamic diagnostic level. + //! + int dynamic_diag_error_level_; + + //! @brief The max (worst) static diagnostic level. + //! + int static_diag_error_level_; + + //! @brief The frequency of the run loop + //! + double frequency_; + + //! @brief What is the acceleration in Z due to gravity (m/s^2)? Default is + //! +9.80665. + //! + double gravitational_acceleration_; + + //! @brief The depth of the history we track for smoothing/delayed measurement + //! processing + //! + //! This is the guaranteed minimum buffer size for which previous states and + //! measurements are kept. + //! + rclcpp::Duration history_length_; + + //! @brief The sensor timeout value that gets passed to the core filter + //! + rclcpp::Duration sensor_timeout_; + + //! @brief tf frame name for the robot's body frame + //! + std::string base_link_frame_id_; + + //! @brief tf frame name for the robot's body frame + //! + //! When the final state is computed, we "override" the output transform and + //! message to have this frame for its child_frame_id. This helps to enable + //! disconnected TF trees when multiple EKF instances are being run. + //! + std::string base_link_output_frame_id_; + + //! @brief tf frame name for the robot's map (world-fixed) frame + //! + std::string map_frame_id_; + + //! @brief tf frame name for the robot's odometry (world-fixed) frame + //! + std::string odom_frame_id_; + + //! @brief tf frame name that is the parent frame of the transform that this + //! node will calculate and broadcast. + //! + std::string world_frame_id_; + + //! @brief Used for outputting debug messages + //! + std::ofstream debug_stream_; + + //! @brief The most recent control input + //! + Eigen::VectorXd latest_control_; + + //! @brief The process noise covariance matrix that gets passed to the core filter + //! + Eigen::MatrixXd process_noise_covariance_; + + //! @brief The initial estimate error covariance matrix that gets passed to the core filter + //! + Eigen::MatrixXd initial_estimate_error_covariance_; + + //! @brief Message that contains our latest transform (i.e., state) + //! + //! We use the vehicle's latest state in a number of places, and often + //! use it as a transform, so this is the most convenient variable to + //! use as our global "current state" object + //! + geometry_msgs::msg::TransformStamped world_base_link_trans_msg_; + + //! @brief last call of periodicUpdate + //! + rclcpp::Time last_diag_time_; + + //! @brief The time of the most recent published state + //! + rclcpp::Time last_published_stamp_; + + //! @brief We process measurements by queueing them up in + //! callbacks and processing them all at once within each + //! iteration + //! + MeasurementQueue measurement_queue_; + + //! @brief Contains the state vector variable names in string format + //! + std::vector state_variable_names_; + + //! @brief This object accumulates dynamic diagnostics, e.g., diagnostics + //! relating to sensor data. + //! + //! The values are considered transient and are cleared at every iteration. + //! + std::map dynamic_diagnostics_; + + //! @brief This object accumulates static diagnostics, e.g., diagnostics + //! relating to the configuration parameters. + //! + //! The values are treated as static and always reported (i.e., this object is + //! never cleared) + //! + std::map static_diagnostics_; + + //! @brief Store the last time a message from each topic was received + //! + //! If we're getting messages rapidly, we may accidentally get + //! an older message arriving after a newer one. This variable keeps + //! track of the most recent message time for each subscribed message + //! topic. We also use it when listening to odometry messages to + //! determine if we should be using messages from that topic. + //! + std::map last_message_times_; + + //! @brief Last time mark that time-differentiation is calculated, in seconds + //! + double last_diff_time_; + + //! @brief Last record of filtered angular velocity + //! + tf2::Vector3 last_state_twist_rot_; + + //! @brief Calculated angular acceleration from time-differencing + //! + tf2::Vector3 angular_acceleration_; + + //! @brief Covariance of the calculated angular acceleration + //! + Eigen::Matrix3d angular_acceleration_cov_; + + //! @brief Stores the first measurement from each topic for relative + //! measurements + //! + //! When a given sensor is being integrated in relative mode, its first + //! measurement is effectively treated as an offset, and future measurements + //! have this first measurement removed before they are fused. This variable + //! stores the initial measurements. Note that this is different from using + //! differential mode, as in differential mode, pose data is converted to + //! twist data, resulting in boundless error growth for the variables being + //! fused. With relative measurements, the vehicle will start with a 0 heading + //! and position, but the measurements are still fused absolutely. + std::map initial_measurements_; + + //! @brief If including acceleration for each IMU input, whether or not we + //! remove acceleration due to gravity + //! + std::map remove_gravitational_acceleration_; + + //! @brief An implicitly time ordered queue of past filter states used for + //! smoothing. + // + // front() refers to the filter state with the earliest timestamp. + // back() refers to the filter state with the latest timestamp. + FilterStateHistoryDeque filter_state_history_; + + //! @brief A deque of previous measurements which is implicitly ordered from + //! earliest to latest measurement. + // when popped from the measurement priority queue. + // front() refers to the measurement with the earliest timestamp. + // back() refers to the measurement with the latest timestamp. + MeasurementHistoryDeque measurement_history_; + + //! @brief Vector to hold our subscribers until they go out of scope + //! + std::vector topic_subs_; + + //! @brief Stores the last measurement from a given topic for differential + //! integration + //! + //! To carry out differential integration, we have to (1) transform + //! that into the target frame (probably the frame specified by + //! @p odomFrameId_), (2) "subtract" the previous measurement from + //! the current measurement, and then (3) transform it again by the previous + //! state estimate. This holds the measurements used for step (2). + //! + std::map previous_measurements_; + + //! @brief We also need the previous covariance matrix for differential data + //! + std::map previous_measurement_covariances_; + + //! @brief By default, the filter predicts and corrects up to the time of the + //! latest measurement. If this is set to true, the filter does the same, but + //! then also predicts up to the current time step. + //! + bool predict_to_current_time_; + + //! @brief Store the last time set pose message was received + //! + //! If we receive a setPose message to reset the filter, we can get in + //! strange situations wherein we process the pose reset, but then even + //! after another spin cycle or two, we can get a new message with a time + //! stamp that is *older* than the setPose message's time stamp. This means + //! we have to check the message's time stamp against the lastSetPoseTime_. + rclcpp::Time last_set_pose_time_; + + //! @brief The time of the most recent control input + //! + rclcpp::Time latest_control_time_; + + //! @brief Parameter that specifies the how long we wait for a transform to + //! become available. + //! + rclcpp::Duration tf_timeout_; + + //! @brief For future (or past) dating the world_frame->base_link_frame + //! transform + //! + rclcpp::Duration tf_time_offset_; + + //! @brief Service that allows another node to toggle on/off filter + //! processing while still publishing. + //! Uses a robot_localization ToggleFilterProcessing service. + //! + rclcpp::Service::SharedPtr + toggle_filter_processing_srv_; + + //! @brief Subscribes to the control input topic + //! + rclcpp::Subscription::SharedPtr control_sub_; + + //! @brief Subscribes to the control stamped input topic + //! + rclcpp::Subscription::SharedPtr stamped_control_sub_; + + //! @brief Subscribes to the set_pose topic (usually published from rviz). + //! Message type is geometry_msgs/PoseWithCovarianceStamped. + //! + rclcpp::Subscription::SharedPtr + set_pose_sub_; + + //! @brief Service that allows another node to change the current state and + //! recieve a confirmation. Uses a custom SetPose service. + //! + rclcpp::Service::SharedPtr + set_pose_service_; + + //! @brief Service that allows another node to enable the filter. Uses a + //! standard Empty service. + //! + rclcpp::Service::SharedPtr enable_filter_srv_; + + //! @brief Service that resets the filter to its initial state + //! + rclcpp::Service::SharedPtr reset_srv_; + + //! @brief Transform buffer for managing coordinate transforms (shared RTTFBuffer singleton) + //! + tf2_ros::Buffer * tf_buffer_{nullptr}; + + //! @brief broadcaster of worldTransform tfs + //! + std::shared_ptr world_transform_broadcaster_; + + //! @brief Used for updating the diagnostics + //! + std::unique_ptr diagnostic_updater_; + + //! @brief Position publisher + //! + rclcpp::Publisher::SharedPtr position_pub_; + + //! Acceleration publisher + //! + rclcpp::Publisher::SharedPtr + accel_pub_; + + //! @brief Our filter (EKF, UKF, etc.) + //! + robot_localization::Ukf filter_; + + //! @brief Timer for filter updates + //! + rclcpp::TimerBase::SharedPtr timer_; + + //! @brief optional signaling diagnostic frequency + //! + std::unique_ptr freq_diag_; + + //! @brief minimum frequency threshold for frequency diagnostic + //! Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam + //! + double min_frequency_; + + //! @brief maximum frequency threshold for frequency diagnostic + //! Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam + //! + double max_frequency_; + + std::string plugin_name_; + std::string tf_prefix_; + std::vector gps_callbackData_arr_; + bool local_filter_; + +}; + +} // namespace robot_localization + +#endif // ROBOT_LOCALIZATION__ROS_FILTER_HPP_ diff --git a/localizers/easynav_fusion_localizer/maps/empty.png b/localizers/easynav_fusion_localizer/maps/empty.png new file mode 100644 index 00000000..cd6eb69a Binary files /dev/null and b/localizers/easynav_fusion_localizer/maps/empty.png differ diff --git a/localizers/easynav_fusion_localizer/maps/empty.yaml b/localizers/easynav_fusion_localizer/maps/empty.yaml new file mode 100644 index 00000000..629ca4cb --- /dev/null +++ b/localizers/easynav_fusion_localizer/maps/empty.yaml @@ -0,0 +1,7 @@ +image: empty.png +mode: trinary +resolution: 0.2 +origin: [-100.0, -100.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/localizers/easynav_fusion_localizer/maps/square.png b/localizers/easynav_fusion_localizer/maps/square.png new file mode 100644 index 00000000..2e6d8527 Binary files /dev/null and b/localizers/easynav_fusion_localizer/maps/square.png differ diff --git a/localizers/easynav_fusion_localizer/maps/square.yaml b/localizers/easynav_fusion_localizer/maps/square.yaml new file mode 100644 index 00000000..34ce4863 --- /dev/null +++ b/localizers/easynav_fusion_localizer/maps/square.yaml @@ -0,0 +1,7 @@ +image: square.png +mode: trinary +resolution: 0.2 +origin: [-100.0, -100.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml new file mode 100644 index 00000000..4c698bcf --- /dev/null +++ b/localizers/easynav_fusion_localizer/package.xml @@ -0,0 +1,42 @@ + + + + easynav_fusion_localizer + 0.4.0 + Easy Navigation: Fusion Localizer package. + Miguel Ángel de Miguel + + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_lifecycle + easynav_core + easynav_common + easynav_localizer + easynav_sensors + pluginlib + geographiclib + geographic_msgs + robot_localization + std_srvs + angles + diagnostic_msgs + diagnostic_updater + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + eigen + + ament_lint_auto + ament_lint_common + robot_localization + + + ament_cmake + + diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp new file mode 100644 index 00000000..b10be598 --- /dev/null +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -0,0 +1,332 @@ +#include "easynav_fusion_localizer/FusionLocalizer.hpp" + +#include "easynav_localizer/LocalizerNode.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_sensors/types/IMUPerception.hpp" +#include "easynav_sensors/types/GNSSPerception.hpp" +#include "sensor_msgs/msg/nav_sat_status.hpp" + +#include + +#include "easynav_common/YTSession.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +namespace easynav +{ + +void FusionLocalizer::on_initialize() +{ + + try { + + auto node = get_node(); + + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&FusionLocalizer::init_pose_callback, this, std::placeholders::_1)); + + auto localizer_node = std::dynamic_pointer_cast(node); + + const std::string & plugin_name = this->get_plugin_name(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + RCLCPP_INFO(localizer_node->get_logger(), "Using tf_prefix: '%s'", tf_info.tf_prefix.c_str()); + RCLCPP_INFO( + localizer_node->get_logger(), "Using parameter namespace: '%s'", + plugin_name.c_str()); + + // Detect which filters have parameters configured by checking + // parameter overrides (loaded from YAML before declaration) + const std::string global_prefix = plugin_name + ".global_filter."; + const std::string local_prefix = plugin_name + ".local_filter."; + + const auto & overrides = + localizer_node->get_node_parameters_interface()->get_parameter_overrides(); + + for (const auto & [key, _] : overrides) { + if (!has_global_filter_ && key.rfind(global_prefix, 0) == 0) { + has_global_filter_ = true; + } + if (!has_local_filter_ && key.rfind(local_prefix, 0) == 0) { + has_local_filter_ = true; + } + if (has_global_filter_ && has_local_filter_) { + break; + } + } + + if (!has_global_filter_ && !has_local_filter_) { + RCLCPP_FATAL( + localizer_node->get_logger(), + "No parameters found for either '%s' or '%s'. " + "At least one filter must be configured.", + global_prefix.c_str(), local_prefix.c_str()); + throw std::runtime_error( + "FusionLocalizer: no global_filter or local_filter parameters detected. " + "At least one filter must be configured."); + } + + if (has_global_filter_) { + ukf_global_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false + ); + ukf_global_->initialize(); + } else { + RCLCPP_WARN( + localizer_node->get_logger(), + "No global_filter parameters found. Global filter will NOT be created."); + } + + if (has_local_filter_) { + ukf_local_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true + ); + ukf_local_->initialize(); + } else { + RCLCPP_WARN( + localizer_node->get_logger(), + "No local_filter parameters found. Local filter will NOT be created."); + } + + // GPS-related setup only needed when global filter is active + if (has_global_filter_) { + localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); + + localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_); + + localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); + + localizer_node->declare_parameter( + plugin_name + ".navsatfix_topic", std::string("gps/filtered")); + localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); + navsat_pub_ = localizer_node->create_publisher( + navsatfix_topic_, rclcpp::QoS(10)); + } + + } catch (const std::exception & e) { + RCLCPP_FATAL( + get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", + e.what()); + throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); + } + + if (has_global_filter_) { + GeographicLib::UTMUPS::Forward( + latitude_origin_, longitude_origin_, UTM_zone_number_, + UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); + UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); + UTM_origin_z_ = altitude_origin_; + + n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); + last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + } + + RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); +} + +void FusionLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + if (has_global_filter_) { + nav_msgs::msg::Odometry global_odom; + // Get current position to calculate the offset + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + double current_x = global_odom.pose.pose.position.x; + double current_y = global_odom.pose.pose.position.y; + + // Shift the UTM origin so that the current GPS position will now map to msg's position + UTM_origin_x_ += (current_x - msg->pose.pose.position.x); + UTM_origin_y_ += (current_y - msg->pose.pose.position.y); + + RCLCPP_INFO( + get_node()->get_logger(), + "Initial pose reset UTM origin. Shifted X by %.2f, Y by %.2f to match pose (%.2f, %.2f)", + (current_x - msg->pose.pose.position.x), (current_y - msg->pose.pose.position.y), + msg->pose.pose.position.x, msg->pose.pose.position.y); + } + + // Forward initial pose to global filter to make the state jump immediately + ukf_global_->setPoseCallback(msg); + first_pose_received_ = true; + } + + if (has_local_filter_) { + ukf_local_->setPoseCallback(msg); + } +} + +void FusionLocalizer::update_rt(NavState & nav_state) +{ + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (has_global_filter_) { + if (n_gps_sensors_ && nav_state.has_group("gnss")) { + auto gps_data = nav_state.get_group(std::string("gnss")); + const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); + for (int i = 0; i < n_gps_sensors_; ++i) { + if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + continue; + } + rclcpp::Time gps_time(gps_data[i]->data.header.stamp); + if (gps_time > last_gps_stamp_[i]) { + EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); + last_gps_stamp_[i] = gps_time; + auto pose = + std::make_shared( + navsatfix_to_pose( + gps_data[i]->data)); + if (!first_pose_received_) { + RCLCPP_INFO( + get_node()->get_logger(), + "First valid GPS fix received. Initializing filter state."); + if (nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + pose->pose.pose.orientation = imu_data.data.orientation; + } + ukf_global_->setPoseCallback(pose); + first_pose_received_ = true; + continue; + } + ukf_global_->poseCallback( + pose, + gps_cb_arr[i], + tf_info.map_frame, + gps_data[i]->data.header.frame_id, + false + ); + } + } + } + + ukf_global_->periodicUpdate(); + + nav_msgs::msg::Odometry global_odom; + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + nav_state.set("robot_pose", global_odom); + navsat_pub_->publish(odom_to_navsatfix(global_odom)); + } + } + + if (has_local_filter_) { + ukf_local_->periodicUpdate(); + + nav_msgs::msg::Odometry local_odom; + if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { + nav_state.set("robot_pose_local", local_odom); + } + } +} + +void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) +{ + +} + +geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose( + const sensor_msgs::msg::NavSatFix & navsat_msg) +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + + pose_msg.header = navsat_msg.header; + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + pose_msg.header.frame_id = tf_info.map_frame; + + double utm_x, utm_y; + int zone; + bool northp; + + GeographicLib::UTMUPS::Forward( + navsat_msg.latitude, + navsat_msg.longitude, + zone, + northp, + utm_x, + utm_y, + UTM_zone_number_); + + pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_; + pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_; + pose_msg.pose.pose.position.z = navsat_msg.altitude - UTM_origin_z_; + + pose_msg.pose.pose.orientation.x = 0.0; + pose_msg.pose.pose.orientation.y = 0.0; + pose_msg.pose.pose.orientation.z = 0.0; + pose_msg.pose.pose.orientation.w = 1.0; + + pose_msg.pose.covariance.fill(0.0); + + double default_var = 1.0; // 1 meter variance standard + + if (navsat_msg.position_covariance_type == + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) + { + pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; + pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; + pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; + + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + } else { + // Fallback variances if GPS doesn't provide them + pose_msg.pose.covariance[0] = default_var; + pose_msg.pose.covariance[7] = default_var; + pose_msg.pose.covariance[14] = default_var; + + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, + "NavSatFix covariance type unknown or invalid. Using default covariance."); + } + + return pose_msg; +} + +sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix( + const nav_msgs::msg::Odometry & odom_msg) +{ + sensor_msgs::msg::NavSatFix navsat_msg; + + navsat_msg.header = odom_msg.header; + navsat_msg.header.frame_id = "gps_link"; + navsat_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + navsat_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + + const double utm_x = odom_msg.pose.pose.position.x + UTM_origin_x_; + const double utm_y = odom_msg.pose.pose.position.y + UTM_origin_y_; + + double latitude = 0.0; + double longitude = 0.0; + GeographicLib::UTMUPS::Reverse( + UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude); + + navsat_msg.latitude = latitude; + navsat_msg.longitude = longitude; + navsat_msg.altitude = odom_msg.pose.pose.position.z + UTM_origin_z_; + + navsat_msg.position_covariance[0] = odom_msg.pose.covariance[0]; + navsat_msg.position_covariance[4] = odom_msg.pose.covariance[7]; + navsat_msg.position_covariance[8] = odom_msg.pose.covariance[14]; + + navsat_msg.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + return navsat_msg; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::FusionLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp new file mode 100644 index 00000000..e4d52a0c --- /dev/null +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -0,0 +1,3862 @@ +/* + * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include "easynav_fusion_localizer/ukf_wrapper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "Eigen/Dense" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/rclcpp.hpp" +#include "robot_localization/ekf.hpp" +#include "robot_localization/filter_common.hpp" +#include "robot_localization/filter_state.hpp" +#include "robot_localization/filter_utilities.hpp" +#include "robot_localization/ros_filter_utilities.hpp" +#include "robot_localization/srv/set_pose.hpp" +#include "robot_localization/srv/toggle_filter_processing.hpp" +#include "robot_localization/ukf.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include +#include + +#include "easynav_common/RTTFBuffer.hpp" + +namespace robot_localization +{ +using namespace std::chrono_literals; + +UkfWrapper::UkfWrapper( + std::shared_ptr parent_node, + const std::string & tf_prefix, + const std::string & plugin_name, + bool local_filter) +: print_diagnostics_(true), + publish_acceleration_(false), + publish_transform_(true), + reset_on_time_jump_(false), + smooth_lagged_data_(false), + toggled_on_(true), + two_d_mode_(false), + use_control_(false), + stamped_control_(true), + disabled_at_startup_(false), + enabled_(false), + permit_corrected_publication_(false), + dynamic_diag_error_level_(diagnostic_msgs::msg::DiagnosticStatus::OK), + static_diag_error_level_(diagnostic_msgs::msg::DiagnosticStatus::OK), + frequency_(30.0), + gravitational_acceleration_(9.80665), + history_length_(0ns), + sensor_timeout_(0ns), + latest_control_(), + process_noise_covariance_(STATE_SIZE, STATE_SIZE), + initial_estimate_error_covariance_(STATE_SIZE, STATE_SIZE), + last_diag_time_(0, 0, RCL_ROS_TIME), + last_published_stamp_(0, 0, RCL_ROS_TIME), + predict_to_current_time_(false), + last_set_pose_time_(0, 0, RCL_ROS_TIME), + latest_control_time_(0, 0, RCL_ROS_TIME), + tf_timeout_(0ns), + tf_time_offset_(0ns), + local_filter_(local_filter) +{ + parent_node_ = parent_node; + tf_prefix_ = tf_prefix; + plugin_name_ = plugin_name; + + // Use the shared RTTFBuffer singleton instead of creating a private tf2_ros::Buffer + tf_buffer_ = easynav::RTTFBuffer::getInstance(); + + state_variable_names_.push_back("X"); + state_variable_names_.push_back("Y"); + state_variable_names_.push_back("Z"); + state_variable_names_.push_back("ROLL"); + state_variable_names_.push_back("PITCH"); + state_variable_names_.push_back("YAW"); + state_variable_names_.push_back("X_VELOCITY"); + state_variable_names_.push_back("Y_VELOCITY"); + state_variable_names_.push_back("Z_VELOCITY"); + state_variable_names_.push_back("ROLL_VELOCITY"); + state_variable_names_.push_back("PITCH_VELOCITY"); + state_variable_names_.push_back("YAW_VELOCITY"); + state_variable_names_.push_back("X_ACCELERATION"); + state_variable_names_.push_back("Y_ACCELERATION"); + state_variable_names_.push_back("Z_ACCELERATION"); +} + +UkfWrapper::~UkfWrapper() +{ + topic_subs_.clear(); + timer_.reset(); + set_pose_sub_.reset(); + control_sub_.reset(); + stamped_control_sub_.reset(); + diagnostic_updater_.reset(); + world_transform_broadcaster_.reset(); + set_pose_service_.reset(); + freq_diag_.reset(); + accel_pub_.reset(); + position_pub_.reset(); +} + +void UkfWrapper::reset() +{ + // Get rid of any initial poses (pretend we've never had a measurement) + initial_measurements_.clear(); + previous_measurements_.clear(); + previous_measurement_covariances_.clear(); + + clearMeasurementQueue(); + + filter_state_history_.clear(); + measurement_history_.clear(); + + angular_acceleration_.setZero(); + angular_acceleration_cov_.setIdentity(); + angular_acceleration_cov_ *= 0.01; + + last_state_twist_rot_.setZero(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + last_set_pose_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + last_diag_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + latest_control_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + last_published_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + last_diff_time_ = parent_node_->now().seconds(); + + // Note: tf_buffer_ is the shared RTTFBuffer singleton, do not clear it here + + // clear last message timestamp, so older messages will be accepted + last_message_times_.clear(); + + // reset filter to uninitialized state + filter_.reset(); + + // Restore filter parameters that we got from the ROS parameter server + filter_.setSensorTimeout(sensor_timeout_); + filter_.setProcessNoiseCovariance(process_noise_covariance_); + filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); +} + +void UkfWrapper::resetSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr) +{ + RCLCPP_INFO( + parent_node_->get_logger(), + "Received a request to reset filter."); + + reset(); +} + +void UkfWrapper::toggleFilterProcessingCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Request> req, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Response> resp) +{ + if (req->on == toggled_on_) { + RCLCPP_WARN( + parent_node_->get_logger(), + "Service was called to toggle filter processing but state was already as " + "requested."); + resp->status = false; + } else { + RCLCPP_INFO( + parent_node_->get_logger(), + "Toggling filter measurement filtering to %s.", req->on ? "On" : "Off"); + toggled_on_ = req->on; + resp->status = true; + } +} + +// @todo: Replace with AccelWithCovarianceStamped +void UkfWrapper::accelerationCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const CallbackData & callback_data, + const std::string & target_frame) +{ + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + return; + } + + const std::string & topic_name = callback_data.topic_name_; + + RF_DEBUG( + "------ UkfWrapper::accelerationCallback (" << topic_name << + ") ------\n") + // "Twist message:\n" << *msg); + + if (last_message_times_.count(topic_name) == 0) { + last_message_times_.insert( + std::pair(topic_name, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (last_message_times_[topic_name] <= msg->header.stamp) { + RF_DEBUG("Update vector for " << topic_name << " is:\n" << topic_name); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurement_covariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector update_vector_corrected = callback_data.update_vector_; + + // Prepare the twist data for inclusion in the filter + if (prepareAcceleration( + msg, topic_name, target_frame, callback_data.relative_, + update_vector_corrected, measurement, + measurement_covariance)) + { + // Store the measurement. Add an "acceleration" suffix so we know what + // kind of measurement we're dealing with when we debug the core filter + // logic. + enqueueMeasurement( + topic_name, measurement, measurement_covariance, + update_vector_corrected, + callback_data.rejection_threshold_, msg->header.stamp); + + RF_DEBUG( + "Enqueued new measurement for " << topic_name << + "_acceleration\n"); + } else { + RF_DEBUG( + "Did *not* enqueue measurement for " << topic_name << + "_acceleration\n"); + } + + last_message_times_[topic_name] = msg->header.stamp; + + RF_DEBUG( + "Last message time for " << + topic_name << " is now " << + filter_utilities::toSec(last_message_times_[topic_name]) << + "\n"); + } else { + // else if (reset_on_time_jump_ && rclcpp::Time::isSimTime()) + //{ + // reset(); + //} + + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp before that of " + "the previous message received," << " this message will be ignored. This may" + " indicate a bad timestamp. (message time: " << msg->header.stamp.nanosec << + ")"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, topic_name + + "_timestamp", stream.str(), false); + + RF_DEBUG( + "Message is too old. Last message time for " << + topic_name << " is " << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << + filter_utilities::toSec(msg->header.stamp) << ".\n"); + } + + RF_DEBUG( + "\n----- /UkfWrapper::accelerationCallback (" << topic_name << + ") ------\n"); +} + +void UkfWrapper::controlCallback( + const geometry_msgs::msg::Twist::SharedPtr msg) +{ + geometry_msgs::msg::TwistStamped::SharedPtr twist_stamped_ptr = + std::make_shared(); + twist_stamped_ptr->twist = *msg; + twist_stamped_ptr->header.frame_id = base_link_frame_id_; + twist_stamped_ptr->header.stamp = parent_node_->now(); + controlStampedCallback(twist_stamped_ptr); +} + +void UkfWrapper::controlStampedCallback( + const geometry_msgs::msg::TwistStamped::SharedPtr msg) +{ + if (msg->header.frame_id == base_link_frame_id_ || + msg->header.frame_id == "") + { + latest_control_(ControlMemberVx) = msg->twist.linear.x; + latest_control_(ControlMemberVy) = msg->twist.linear.y; + latest_control_(ControlMemberVz) = msg->twist.linear.z; + latest_control_(ControlMemberVroll) = msg->twist.angular.x; + latest_control_(ControlMemberVpitch) = msg->twist.angular.y; + latest_control_(ControlMemberVyaw) = msg->twist.angular.z; + latest_control_time_ = msg->header.stamp; + + // Update the filter with this control term + filter_.setControl(latest_control_, msg->header.stamp); + } else { + RCLCPP_WARN_STREAM_THROTTLE( + parent_node_->get_logger(), *parent_node_->get_clock(), 5000, "Commanded velocities " + " must be given in the robot's body frame (" << base_link_frame_id_ << + "). Message frame was " << msg->header.frame_id); + } +} + +void UkfWrapper::enqueueMeasurement( + const std::string & topic_name, const Eigen::VectorXd & measurement, + const Eigen::MatrixXd & measurement_covariance, + const std::vector & update_vector, const double mahalanobis_thresh, + const rclcpp::Time & time) +{ + MeasurementPtr meas = std::make_shared(); + + meas->topic_name_ = topic_name; + meas->measurement_ = measurement; + meas->covariance_ = measurement_covariance; + meas->update_vector_ = update_vector; + meas->time_ = time; + meas->mahalanobis_thresh_ = mahalanobis_thresh; + meas->latest_control_ = latest_control_; + meas->latest_control_time_ = latest_control_time_; + measurement_queue_.push(meas); +} + +void UkfWrapper::forceTwoD( + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance, + std::vector & update_vector) +{ + // Force 3D variables to 0 in the measurement + measurement(StateMemberZ) = 0.0; + measurement(StateMemberRoll) = 0.0; + measurement(StateMemberPitch) = 0.0; + measurement(StateMemberVz) = 0.0; + measurement(StateMemberVroll) = 0.0; + measurement(StateMemberVpitch) = 0.0; + measurement(StateMemberAz) = 0.0; + + // Need to eliminate any off-diagonal covariance values that involve one of our 3D variables + measurement_covariance.col(StateMemberZ).fill(0.0); + measurement_covariance.col(StateMemberRoll).fill(0.0); + measurement_covariance.col(StateMemberPitch).fill(0.0); + measurement_covariance.col(StateMemberVz).fill(0.0); + measurement_covariance.col(StateMemberVroll).fill(0.0); + measurement_covariance.col(StateMemberVpitch).fill(0.0); + measurement_covariance.col(StateMemberAz).fill(0.0); + + measurement_covariance.row(StateMemberZ).fill(0.0); + measurement_covariance.row(StateMemberRoll).fill(0.0); + measurement_covariance.row(StateMemberPitch).fill(0.0); + measurement_covariance.row(StateMemberVz).fill(0.0); + measurement_covariance.row(StateMemberVroll).fill(0.0); + measurement_covariance.row(StateMemberVpitch).fill(0.0); + measurement_covariance.row(StateMemberAz).fill(0.0); + + // Now set the diagonal covariance values to something small + measurement_covariance(StateMemberZ, StateMemberZ) = 1e-6; + measurement_covariance(StateMemberRoll, StateMemberRoll) = 1e-6; + measurement_covariance(StateMemberPitch, StateMemberPitch) = 1e-6; + measurement_covariance(StateMemberVz, StateMemberVz) = 1e-6; + measurement_covariance(StateMemberVroll, StateMemberVroll) = 1e-6; + measurement_covariance(StateMemberVpitch, StateMemberVpitch) = 1e-6; + measurement_covariance(StateMemberAz, StateMemberAz) = 1e-6; + + // Finally, update the update vector + update_vector[StateMemberZ] = 1; + update_vector[StateMemberRoll] = 1; + update_vector[StateMemberPitch] = 1; + update_vector[StateMemberVz] = 1; + update_vector[StateMemberVroll] = 1; + update_vector[StateMemberVpitch] = 1; + update_vector[StateMemberAz] = 1; +} + +bool UkfWrapper::getFilteredOdometryMessage(nav_msgs::msg::Odometry * message) +{ + // If the filter has received a measurement at some point... + if (filter_.getInitializedStatus()) { + // Grab our current state and covariance estimates + const Eigen::VectorXd & state = filter_.getState(); + const Eigen::MatrixXd & estimate_error_covariance = + filter_.getEstimateErrorCovariance(); + + // Convert from roll, pitch, and yaw back to quaternion for + // orientation values + tf2::Quaternion quat; + quat.setRPY( + state(StateMemberRoll), state(StateMemberPitch), + state(StateMemberYaw)); + + // Fill out the message + message->pose.pose.position.x = state(StateMemberX); + message->pose.pose.position.y = state(StateMemberY); + message->pose.pose.position.z = state(StateMemberZ); + message->pose.pose.orientation.x = quat.x(); + message->pose.pose.orientation.y = quat.y(); + message->pose.pose.orientation.z = quat.z(); + message->pose.pose.orientation.w = quat.w(); + message->twist.twist.linear.x = state(StateMemberVx); + message->twist.twist.linear.y = state(StateMemberVy); + message->twist.twist.linear.z = state(StateMemberVz); + message->twist.twist.angular.x = state(StateMemberVroll); + message->twist.twist.angular.y = state(StateMemberVpitch); + message->twist.twist.angular.z = state(StateMemberVyaw); + + // Our covariance matrix layout doesn't quite match + for (size_t i = 0; i < POSE_SIZE; i++) { + for (size_t j = 0; j < POSE_SIZE; j++) { + message->pose.covariance[POSE_SIZE * i + j] = + estimate_error_covariance(i, j); + } + } + + // POSE_SIZE and TWIST_SIZE are currently the same size, but we can spare a + // few cycles to be meticulous and not index a twist covariance array on the + // size of a pose covariance array + for (size_t i = 0; i < TWIST_SIZE; i++) { + for (size_t j = 0; j < TWIST_SIZE; j++) { + message->twist.covariance[TWIST_SIZE * i + j] = + estimate_error_covariance( + i + POSITION_V_OFFSET, + j + POSITION_V_OFFSET); + } + } + + message->header.stamp = filter_.getLastMeasurementTime(); + message->header.frame_id = world_frame_id_; + message->child_frame_id = base_link_output_frame_id_; + } + + return filter_.getInitializedStatus(); +} + +bool UkfWrapper::getFilteredAccelMessage( + geometry_msgs::msg::AccelWithCovarianceStamped * message) +{ + // If the filter has received a measurement at some point... + if (filter_.getInitializedStatus()) { + // Grab our current state and covariance estimates + const Eigen::VectorXd & state = filter_.getState(); + const Eigen::MatrixXd & estimate_error_covariance = + filter_.getEstimateErrorCovariance(); + + //! Fill out the accel_msg + message->accel.accel.linear.x = state(StateMemberAx); + message->accel.accel.linear.y = state(StateMemberAy); + message->accel.accel.linear.z = state(StateMemberAz); + message->accel.accel.angular.x = angular_acceleration_.x(); + message->accel.accel.angular.y = angular_acceleration_.y(); + message->accel.accel.angular.z = angular_acceleration_.z(); + + // Fill the covariance (only the left-upper matrix since we are not + // estimating the rotational accelerations arround the axes + for (size_t i = 0; i < ACCELERATION_SIZE; i++) { + for (size_t j = 0; j < ACCELERATION_SIZE; j++) { + // We use the POSE_SIZE since the accel cov matrix of ROS is 6x6 + message->accel.covariance[POSE_SIZE * i + j] = estimate_error_covariance( + i + POSITION_A_OFFSET, j + POSITION_A_OFFSET); + } + } + for (size_t i = ACCELERATION_SIZE; i < POSE_SIZE; i++) { + for (size_t j = ACCELERATION_SIZE; j < POSE_SIZE; j++) { + // fill out the angular portion. We assume the linear and angular portions are independent. + message->accel.covariance[POSE_SIZE * i + j] = + angular_acceleration_cov_(i - ACCELERATION_SIZE, j - ACCELERATION_SIZE); + } + } + + // Fill header information + message->header.stamp = rclcpp::Time(filter_.getLastMeasurementTime()); + message->header.frame_id = base_link_output_frame_id_; + } + + return filter_.getInitializedStatus(); +} + +void UkfWrapper::imuCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data, + const CallbackData & accel_callback_data) +{ + RF_DEBUG( + "------ UkfWrapper::imuCallback (" << + topic_name << ") ------\n") // << "IMU message:\n" << *msg); + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp equal to or" + " before the last filter reset, " << "this message will be ignored. This may" + "indicate an empty or bad timestamp. (message time: " << msg->header.stamp.nanosec << + ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + + + RF_DEBUG( + "Received message that preceded the most recent pose reset. " + "Ignoring..."); + + return; + } + + // As with the odometry message, we can separate out the pose- and + // twist-related variables in the IMU message and pass them to the pose and + // twist callbacks (filters) + if (pose_callback_data.update_sum_ > 0) { + // Per the IMU message specification, if the IMU does not provide + // orientation, then its first covariance value should be set to -1, and we + // should ignore that portion of the message. robot_localization allows + // users to explicitly ignore data using its parameters, but we should also + // be compliant with message specs. + if (std::abs(msg->orientation_covariance[0] + 1) < 1e-9) { + RF_DEBUG( + "Received IMU message with -1 as its first covariance value for " + "orientation. " + "Ignoring orientation..."); + } else { + // Extract the pose (orientation) data, pass it to its filter + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pos_ptr = + std::make_shared(); + pos_ptr->header = msg->header; + pos_ptr->pose.pose.orientation = msg->orientation; + + // Copy the covariance for roll, pitch, and yaw + for (size_t i = 0; i < ORIENTATION_SIZE; i++) { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) { + pos_ptr->pose.covariance[POSE_SIZE * (i + ORIENTATION_SIZE) + + (j + ORIENTATION_SIZE)] = + msg->orientation_covariance[ORIENTATION_SIZE * i + j]; + } + } + + // IMU data gets handled a bit differently, since the message is ambiguous + // and has only a single frame_id, even though the data in it is reported + // in two different frames. As we assume users will specify a base_link to + // imu transform, we make the target and child frame base_link_frame_id_ and + // tell the poseCallback that it is working with IMU data. This will cause + // it to apply different logic to the data. + poseCallback( + pos_ptr, pose_callback_data, base_link_frame_id_, + base_link_frame_id_, true); + } + } + + if (twist_callback_data.update_sum_ > 0) { + // Ignore rotational velocity if the first covariance value is -1 + if (std::abs(msg->angular_velocity_covariance[0] + 1) < 1e-9) { + RF_DEBUG( + "Received IMU message with -1 as its first covariance value for " + "angular " + "velocity. Ignoring angular velocity..."); + } else { + // Repeat for velocity + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist_ptr = + std::make_shared(); + twist_ptr->header = msg->header; + twist_ptr->twist.twist.angular = msg->angular_velocity; + + // Copy the covariance + for (size_t i = 0; i < ORIENTATION_SIZE; i++) { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) { + twist_ptr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + + (j + ORIENTATION_SIZE)] = + msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j]; + } + } + + twistCallback(twist_ptr, twist_callback_data, base_link_frame_id_); + } + } + + if (accel_callback_data.update_sum_ > 0) { + // Ignore linear acceleration if the first covariance value is -1 + if (std::abs(msg->linear_acceleration_covariance[0] + 1) < 1e-9) { + RF_DEBUG( + "Received IMU message with -1 as its first covariance value for " + "linear " + "acceleration. Ignoring linear acceleration..."); + } else { + // Pass the message on + accelerationCallback(msg, accel_callback_data, base_link_frame_id_); + } + } + + RF_DEBUG("\n----- /UkfWrapper::imuCallback (" << topic_name << ") ------\n"); +} + +void UkfWrapper::integrateMeasurements(const rclcpp::Time & current_time) +{ + RF_DEBUG( + "------ UkfWrapper::integrateMeasurements ------\n\n" + "Integration time is " << + std::setprecision(20) << filter_utilities::toSec(current_time) << + "\n" << + measurement_queue_.size() << " measurements in queue.\n"); + + bool predict_to_current_time = predict_to_current_time_; + + // If we have any measurements in the queue, process them + if (!measurement_queue_.empty()) { + // Check if the first measurement we're going to process is older than the + // filter's last measurement. This means we have received an out-of-sequence + // message (one with an old timestamp), and we need to revert both the + // filter state and measurement queue to the first state that preceded the + // time stamp of our first measurement. + const MeasurementPtr & first_measurement = measurement_queue_.top(); + int restored_measurement_count = 0; + if (smooth_lagged_data_ && + first_measurement->time_ < filter_.getLastMeasurementTime()) + { + RF_DEBUG( + "Received a measurement that was " << + filter_utilities::toSec( + filter_.getLastMeasurementTime() - + first_measurement->time_) << + " seconds in the past. Reverting filter state and " + "measurement queue..."); + + int original_count = static_cast(measurement_queue_.size()); + const rclcpp::Time first_measurement_time = first_measurement->time_; + const std::string first_measurement_topic = + first_measurement->topic_name_; + // revertTo may invalidate first_measurement + if (!revertTo(first_measurement_time - rclcpp::Duration(1ns))) { + RF_DEBUG( + "ERROR: history interval is too small to revert to time " << + filter_utilities::toSec(first_measurement_time) << "\n"); + // ROS_WARN_STREAM_DELAYED_THROTTLE(history_length_, + // "Received old measurement for topic " << first_measurement_topic << + // ", but history interval is insufficiently sized. " + // "Measurement time is " << std::setprecision(20) << + // first_measurement_time << + // ", current time is " << current_time << + // ", history length is " << history_length_ << "."); + restored_measurement_count = 0; + } + + restored_measurement_count = + static_cast(measurement_queue_.size()) - original_count; + } + + while (!measurement_queue_.empty() && rclcpp::ok()) { + MeasurementPtr measurement = measurement_queue_.top(); + + // If we've reached a measurement that has a time later than now, it + // should wait until a future iteration. Since measurements are stored in + // a priority queue, all remaining measurements will be in the future. + if (current_time < measurement->time_) { + break; + } + + measurement_queue_.pop(); + + // When we receive control messages, we call this directly in the control + // callback. However, we also associate a control with each sensor message + // so that we can support lagged smoothing. As we cannot guarantee that + // the new control callback will fire before a new measurement, we should + // only perform this operation if we are processing messages from the + // history. Otherwise, we may get a new measurement, store the "old" + // latest control, then receive a control, call setControl, and then + // overwrite that value with this one (i.e., with the "old" control we + // associated with the measurement). + if (use_control_ && restored_measurement_count > 0) { + filter_.setControl( + measurement->latest_control_, + measurement->latest_control_time_); + restored_measurement_count--; + } + + auto previous_state = filter_.getState(); + auto previous_covar = filter_.getEstimateErrorCovariance(); + auto last_measurement_time = filter_.getLastMeasurementTime(); + + // This will call predict and, if necessary, correct + filter_.processMeasurement(*(measurement.get())); + + // Store old states and measurements if we're smoothing + if (smooth_lagged_data_) { + // Invariant still holds: measurementHistoryDeque_.back().time_ < + // measurement_queue_.top().time_ + measurement_history_.push_back(measurement); + + // We should only save the filter state once per unique timstamp + if (measurement_queue_.empty() || + measurement_queue_.top()->time_ != + filter_.getLastMeasurementTime()) + { + saveFilterState(filter_); + } + } + } + } else if (filter_.getInitializedStatus()) { + // In the event that we don't get any measurements for a long time, + // we still need to continue to estimate our state. Therefore, we + // should project the state forward here. + rclcpp::Duration last_update_delta = + current_time - filter_.getLastMeasurementTime(); + + // If we get a large delta, then continuously predict until + if (last_update_delta >= filter_.getSensorTimeout()) { + predict_to_current_time = true; + + RF_DEBUG( + "Sensor timeout! Last measurement time was " << + filter_utilities::toSec(filter_.getLastMeasurementTime()) << + ", current time is " << filter_utilities::toSec(current_time) << + ", delta is " << filter_utilities::toSec(last_update_delta) << + "\n"); + } + } else { + RF_DEBUG("Filter not yet initialized.\n"); + } + + if (filter_.getInitializedStatus() && predict_to_current_time) { + rclcpp::Duration last_update_delta = + current_time - filter_.getLastMeasurementTime(); + + filter_.validateDelta(last_update_delta); + filter_.predict(current_time, last_update_delta); + + // Update the last measurement time and last update time + filter_.setLastMeasurementTime( + filter_.getLastMeasurementTime() + + last_update_delta); + } + + RF_DEBUG("\n----- /UkfWrapper::integrateMeasurements ------\n"); +} + +void UkfWrapper::differentiateMeasurements(const rclcpp::Time & current_time) +{ + if (filter_.getInitializedStatus()) { + const double time_now = filter_utilities::toSec(current_time); + const double dt = time_now - last_diff_time_; + const Eigen::VectorXd & state = filter_.getState(); + tf2::Vector3 new_state_twist_rot( + state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + angular_acceleration_ = (new_state_twist_rot - last_state_twist_rot_) / dt; + const Eigen::MatrixXd & cov = filter_.getEstimateErrorCovariance(); + for (size_t i = 0; i < ORIENTATION_SIZE; i++) { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) { + angular_acceleration_cov_(i, j) = + cov(i + ORIENTATION_V_OFFSET, j + ORIENTATION_V_OFFSET) * 2. / + ( dt * dt ); + } + } + last_state_twist_rot_ = new_state_twist_rot; + last_diff_time_ = time_now; + } +} + +void UkfWrapper::loadParams() +{ + // Get the plugin name, if any + const std::string param_prefix = plugin_name_.empty() ? "" : plugin_name_ + "."; + + // Get callback_group + auto rt_cbg = parent_node_->get_real_time_cbg(); + rclcpp::SubscriptionOptions options; + options.callback_group = rt_cbg; + + double alpha = parent_node_->declare_parameter(param_prefix + "alpha", 0.001); + double kappa = parent_node_->declare_parameter(param_prefix + "kappa", 0.0); + double beta = parent_node_->declare_parameter(param_prefix + "beta", 2.0); + filter_.setConstants(alpha, kappa, beta); + + /* For diagnostic purposes, collect information about how many different + * sources are measuring each absolute pose variable and do not have + * differential integration enabled. + */ + std::map abs_pose_var_counts; + abs_pose_var_counts[StateMemberX] = 0; + abs_pose_var_counts[StateMemberY] = 0; + abs_pose_var_counts[StateMemberZ] = 0; + abs_pose_var_counts[StateMemberRoll] = 0; + abs_pose_var_counts[StateMemberPitch] = 0; + abs_pose_var_counts[StateMemberYaw] = 0; + + // Same for twist variables + std::map twist_var_counts; + twist_var_counts[StateMemberVx] = 0; + twist_var_counts[StateMemberVy] = 0; + twist_var_counts[StateMemberVz] = 0; + twist_var_counts[StateMemberVroll] = 0; + twist_var_counts[StateMemberVpitch] = 0; + twist_var_counts[StateMemberVyaw] = 0; + + // Determine if we'll be printing diagnostic information + print_diagnostics_ = parent_node_->declare_parameter(param_prefix + "print_diagnostics", false); + + // Check for custom gravitational acceleration value + gravitational_acceleration_ = parent_node_->declare_parameter( + param_prefix + + "gravitational_acceleration", + gravitational_acceleration_); + + // Grab the debug param. If true, the node will produce a LOT of output. + bool debug = parent_node_->declare_parameter(param_prefix + "debug", false); + std::string debug_out_file = "robot_localization_debug.txt"; + if (debug) { + try { + debug_out_file = parent_node_->declare_parameter( + param_prefix + "debug_out_file", + debug_out_file); + debug_stream_.open(debug_out_file.c_str()); + + // Make sure we succeeded + if (debug_stream_.is_open()) { + filter_.setDebug(debug, &debug_stream_); + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "UkfWrapper::loadParams() - unable to create debug output file " << debug_out_file); + } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "UkfWrapper::loadParams() - unable to create debug output file " << debug_out_file << + ". Error was " << e.what()); + } + } + + // Frame configuration comes from Easynav TFInfo; no frame params declared here + + const auto & tf_info = easynav::RTTFBuffer::getInstance()->get_tf_info(); + + map_frame_id_ = tf_info.map_frame; + odom_frame_id_ = tf_info.odom_frame; + base_link_frame_id_ = tf_info.robot_frame; + // World frame comes from Easynav TFInfo configuration + if (local_filter_) { + world_frame_id_ = tf_info.odom_frame; + } else { + world_frame_id_ = tf_info.map_frame; + } + + base_link_output_frame_id_ = base_link_frame_id_; + + /* + * These parameters are designed to enforce compliance with REP-105: + * http://www.ros.org/reps/rep-0105.html + * When fusing absolute position data from sensors such as GPS, the state + * estimate can undergo discrete jumps. According to REP-105, we want three + * coordinate frames: map, odom, and base_link. The map frame can have + * discontinuities, but is the frame with the most accurate position estimate + * for the robot and should not suffer from drift. The odom frame drifts over + * time, but is guaranteed to be continuous and is accurate enough for local + * planning and navigation. The base_link frame is affixed to the robot. The + * intention is that some odometry source broadcasts the odom->base_link + * transform. The localization software should broadcast map->base_link. + * However, tf does not allow multiple parents for a coordinate frame, so + * we must *compute* map->base_link, but then use the existing odom->base_link + * transform to compute *and broadcast* map->odom. + * + * The state estimation nodes in robot_localization therefore have two + * "modes." If your world_frame parameter value matches the map_frame + * parameter value, then robot_localization will assume someone else is + * broadcasting a transform from odom_frame->base_link_frame, and it will + * compute the map_frame->odom_frame transform. Otherwise, it will simply + * compute the odom_frame->base_link_frame transform. + * + * The default is the latter behavior (broadcast of odom->base_link). + */ + + + if (map_frame_id_ == odom_frame_id_ || + odom_frame_id_ == base_link_frame_id_ || + map_frame_id_ == base_link_frame_id_ || + odom_frame_id_ == base_link_output_frame_id_ || + map_frame_id_ == base_link_output_frame_id_) + { + RCLCPP_ERROR( + parent_node_->get_logger(), + "Invalid frame configuration! The values for map_frame, " + "odom_frame, and base_link_frame must be unique. If using a base_link_frame_output " + "values, it must not match the map_frame or odom_frame."); + } + + if (!tf_prefix_.empty()) { + // Append the tf prefix in a tf2-friendly manner + filter_utilities::appendPrefix(tf_prefix_, map_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, odom_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, base_link_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, base_link_output_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, world_frame_id_); + } + + // Whether we're publshing the world_frame->base_link_frame transform + publish_transform_ = parent_node_->declare_parameter(param_prefix + "publish_tf", true); + + // Whether we're publishing the acceleration state transform + publish_acceleration_ = parent_node_->declare_parameter( + param_prefix + "publish_acceleration", + false); + + // Whether we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication_ = parent_node_->declare_parameter( + param_prefix + + "permit_corrected_publication", false); + + // Transform future dating + double offset_tmp = parent_node_->declare_parameter(param_prefix + "transform_time_offset", 0.0); + tf_time_offset_ = rclcpp::Duration::from_seconds(offset_tmp); + + // Transform timeout + double timeout_tmp = parent_node_->declare_parameter(param_prefix + "transform_timeout", 0.0); + tf_timeout_ = rclcpp::Duration::from_seconds(timeout_tmp); + + // Update frequency and sensor timeout + frequency_ = parent_node_->declare_parameter(param_prefix + "frequency", 30.0); + + predict_to_current_time_ = parent_node_->declare_parameter( + param_prefix + + "predict_to_current_time", false); + + sensor_timeout_ = + rclcpp::Duration::from_seconds( + parent_node_->declare_parameter( + param_prefix + "sensor_timeout", + 1.0 / frequency_)); + filter_.setSensorTimeout(sensor_timeout_); + + // Determine if we're in 2D mode + two_d_mode_ = parent_node_->declare_parameter(param_prefix + "two_d_mode", false); + + // Smoothing window size + smooth_lagged_data_ = parent_node_->declare_parameter(param_prefix + "smooth_lagged_data", false); + double history_length_double = parent_node_->declare_parameter( + param_prefix + "history_length", + 0.0); + + if (!smooth_lagged_data_ && std::abs(history_length_double) > 0) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Filter history interval of " << history_length_double << + " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed."); + } + + if (smooth_lagged_data_ && history_length_double < 0) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Negative history interval of " << history_length_double << " specified. Absolute value " + "will be assumed."); + } + + history_length_ = rclcpp::Duration::from_seconds(std::abs(history_length_double)); + + // Whether we reset filter on jump back in time + reset_on_time_jump_ = parent_node_->declare_parameter(param_prefix + "reset_on_time_jump", false); + + // Determine if we're using a control term + double control_timeout = sensor_timeout_.seconds(); + std::vector control_update_vector; + std::vector acceleration_limits; + std::vector acceleration_gains; + std::vector deceleration_limits; + std::vector deceleration_gains; + + use_control_ = parent_node_->declare_parameter(param_prefix + "use_control", false); + stamped_control_ = parent_node_->declare_parameter(param_prefix + "stamped_control", true); + control_timeout = parent_node_->declare_parameter(param_prefix + "control_timeout", 0.0); + + if (use_control_) { + parent_node_->declare_parameter(param_prefix + "control_config", rclcpp::PARAMETER_BOOL_ARRAY); + if (parent_node_->get_parameter(param_prefix + "control_config", control_update_vector)) { + if (control_update_vector.size() != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Control configuration must be of size " << + TWIST_SIZE << ". Provided config was of size " << control_update_vector.size() << + ". No control term will be used."); + use_control_ = false; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but control_config is missing. No control term will be " + "used."); + use_control_ = false; + } + + parent_node_->declare_parameter( + param_prefix + "acceleration_limits", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "acceleration_limits", acceleration_limits)) { + if (acceleration_limits.size() != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Acceleration configuration must be of size " << TWIST_SIZE << + ". Provided config was of size " << acceleration_limits.size() << + ". No control term will be used."); + use_control_ = false; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but acceleration_limits is missing. Will use default " + "values."); + acceleration_limits.resize(TWIST_SIZE, 1.0); + } + + parent_node_->declare_parameter( + param_prefix + "acceleration_gains", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "acceleration_gains", acceleration_gains)) { + const int size = acceleration_gains.size(); + if (size != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Acceleration gain configuration must be of size " << TWIST_SIZE << ". Provided config " + "was of size " << size << ". All gains will be assumed to be 1."); + std::fill_n( + acceleration_gains.begin(), std::min(size, TWIST_SIZE), + 1.0); + acceleration_gains.resize(TWIST_SIZE, 1.0); + } + } + + parent_node_->declare_parameter( + param_prefix + "deceleration_limits", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "deceleration_limits", deceleration_limits)) { + if (deceleration_limits.size() != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Deceleration configuration must be of size " << TWIST_SIZE << ". Provided config was " + "of size " << deceleration_limits.size() << ". No control term will be used."); + use_control_ = false; + } + } else { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but no deceleration_limits specified. Will use acceleration " + "limits."); + deceleration_limits = acceleration_limits; + } + + parent_node_->declare_parameter( + param_prefix + "deceleration_gains", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "deceleration_gains", deceleration_gains)) { + const int size = deceleration_gains.size(); + if (size != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Deceleration gain configuration must be of size " << TWIST_SIZE << ". Provided config " + "was of size " << size << ". All gains will be assumed to be 1."); + std::fill_n( + deceleration_gains.begin(), std::min(size, TWIST_SIZE), + 1.0); + deceleration_gains.resize(TWIST_SIZE, 1.0); + } + } else { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but no deceleration_gains specified. Will use acceleration " + "gains."); + deceleration_gains = acceleration_gains; + } + } else { + control_update_vector.resize(TWIST_SIZE, 0); + acceleration_limits.resize(TWIST_SIZE, 1.0); + acceleration_gains.resize(TWIST_SIZE, 1.0); + deceleration_limits.resize(TWIST_SIZE, 1.0); + deceleration_gains.resize(TWIST_SIZE, 1.0); + } + + bool dynamic_process_noise_covariance = parent_node_->declare_parameter( + param_prefix + + "dynamic_process_noise_covariance", false); + filter_.setUseDynamicProcessNoiseCovariance( + dynamic_process_noise_covariance); + + std::vector initial_state; + parent_node_->declare_parameter(param_prefix + "initial_state", rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "initial_state", initial_state)) { + if (initial_state.size() != STATE_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Initial state must be of size " << STATE_SIZE << ". Provided config was of size " << + initial_state.size() << ". The initial state will be ignored."); + } else { + Eigen::Map eigen_state(initial_state.data(), + initial_state.size()); + filter_.setState(eigen_state); + } + } + + // Check if the filter should start or not + disabled_at_startup_ = parent_node_->declare_parameter( + param_prefix + "disabled_at_startup", + false); + enabled_ = !disabled_at_startup_; + + // Debugging writes to file + RF_DEBUG( + std::boolalpha << + "tf_prefix is " << tf_prefix_ << + "\nmap_frame is " << map_frame_id_ << + "\nodom_frame is " << odom_frame_id_ << + "\nbase_link_frame is " << base_link_frame_id_ << + "\nbase_link_output_frame is " << base_link_output_frame_id_ << + "\nworld_frame is " << world_frame_id_ << + "\ntransform_time_offset is " << filter_utilities::toSec(tf_time_offset_) << + "\ntransform_timeout is " << filter_utilities::toSec(tf_timeout_) << + "\nfrequency is " << frequency_ << + "\nsensor_timeout is " << filter_utilities::toSec(filter_.getSensorTimeout()) << + "\ntwo_d_mode is " << (two_d_mode_ ? "true" : "false") << + "\nsmooth_lagged_data is " << (smooth_lagged_data_ ? "true" : "false") << + "\nhistory_length is " << filter_utilities::toSec(history_length_) << + "\nuse_control is " << use_control_ << + "\nstamped_control_is " << stamped_control_ << + "\ncontrol_config is " << control_update_vector << + "\ncontrol_timeout is " << control_timeout << + "\nacceleration_limits are " << acceleration_limits << + "\nacceleration_gains are " << acceleration_gains << + "\ndeceleration_limits are " << deceleration_limits << + "\ndeceleration_gains are " << deceleration_gains << + "\ninitial state is " << filter_.getState() << + "\ndynamic_process_noise_covariance is " << dynamic_process_noise_covariance << + "\npermit_corrected_publication is " << permit_corrected_publication_ << + "\nprint_diagnostics is " << print_diagnostics_ << "\n"); + + // Determine mode prefix for service/topic names to avoid collisions + // between local and global filter instances + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } + + // Create a subscriber for manually setting/resetting pose + set_pose_sub_ = + parent_node_->create_subscription( + mode_prefix + "set_pose", rclcpp::QoS(1), + std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options); + + // Create a service for manually setting/resetting pose + set_pose_service_ = + parent_node_->create_service( + mode_prefix + "set_pose", std::bind( + &UkfWrapper::setPoseSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Create a service for manually enabling the filter + enable_filter_srv_ = + parent_node_->create_service( + "~/" + mode_prefix + "enable", std::bind( + &UkfWrapper::enableFilterSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Create a service for manually setting/resetting pose + reset_srv_ = + parent_node_->create_service( + mode_prefix + "reset", std::bind( + &UkfWrapper::resetSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Create a service for toggling processing new measurements while still + // publishing + toggle_filter_processing_srv_ = + parent_node_->create_service( + "~/" + mode_prefix + "toggle", std::bind( + &UkfWrapper::toggleFilterProcessingCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Init the last measurement time so we don't get a huge initial delta + filter_.setLastMeasurementTime(parent_node_->now()); + + // Now pull in each topic to which we want to subscribe. + // Start with odom. + size_t topic_ind = 0; + bool more_params = false; + do { + // Build the string in the form of "odomX", where X is the odom topic + // number, then check if we have any parameters with that value. Users need + // to make sure they don't have gaps in their configs (e.g., odom0 and then + // odom2) + std::stringstream ss; + ss << "odom" << topic_ind++; + std::string odom_topic_name = ss.str(); + std::string odom_topic; + parent_node_->declare_parameter(param_prefix + odom_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + odom_topic_name, parameter)) { + more_params = true; + odom_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + // Determine if we want to integrate this sensor differentially + bool differential = parent_node_->declare_parameter( + param_prefix + + odom_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter( + param_prefix + odom_topic_name + + std::string("_relative"), false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << odom_topic_name << "_differential" << " and " << odom_topic_name << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Consider odometry transformation from the child_frame_id instead of the base_link_frame_id + bool pose_use_child_frame = parent_node_->declare_parameter( + param_prefix + + odom_topic_name + std::string("_pose_use_child_frame"), false); + + // Check for pose rejection threshold + double pose_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + odom_topic_name + + std::string("_pose_rejection_threshold"), + std::numeric_limits::max()); + + // Check for twist rejection threshold + double twist_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + odom_topic_name + + std::string("_twist_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter( + param_prefix + + odom_topic_name + + std::string("_queue_size"), 10); + + // Now pull in its boolean update vector configuration. Create separate + // vectors for pose and twist data, and then zero out the opposite values + // in each vector (no pose data in the twist update vector and + // vice-versa). + std::vector update_vec = loadUpdateConfig(odom_topic_name); + std::vector pose_update_vec = update_vec; + std::fill( + pose_update_vec.begin() + POSITION_V_OFFSET, + pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::vector twist_update_vec = update_vec; + std::fill( + twist_update_vec.begin() + POSITION_OFFSET, + twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + + int pose_update_sum = + std::accumulate(pose_update_vec.begin(), pose_update_vec.end(), 0); + int twist_update_sum = + std::accumulate(twist_update_vec.begin(), twist_update_vec.end(), 0); + + const CallbackData pose_callback_data( + odom_topic_name + "_pose", pose_update_vec, pose_update_sum, + differential, relative, pose_use_child_frame, pose_mahalanobis_thresh); + + const CallbackData twist_callback_data( + odom_topic_name + "_twist", twist_update_vec, twist_update_sum, false, + false, false, twist_mahalanobis_thresh); + + // Store the odometry topic subscribers so they don't go out of scope. + if (pose_update_sum + twist_update_sum > 0) { + std::function)> + odom_callback = std::bind( + &UkfWrapper::odometryCallback, this, + std::placeholders::_1, odom_topic_name, + pose_callback_data, twist_callback_data); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + topic_subs_.push_back( + parent_node_->create_subscription( + odom_topic, custom_qos, + odom_callback, options)); + } else { + std::stringstream stream; + stream << odom_topic << " is listed as an input topic, but all update " + "variables are false"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + odom_topic + "_configuration", stream.str(), true); + } + + if (pose_update_sum > 0) { + if (differential) { + twist_var_counts[StateMemberVx] += pose_update_vec[StateMemberX]; + twist_var_counts[StateMemberVy] += pose_update_vec[StateMemberY]; + twist_var_counts[StateMemberVz] += pose_update_vec[StateMemberZ]; + twist_var_counts[StateMemberVroll] += + pose_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + pose_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += pose_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberX] += pose_update_vec[StateMemberX]; + abs_pose_var_counts[StateMemberY] += pose_update_vec[StateMemberY]; + abs_pose_var_counts[StateMemberZ] += pose_update_vec[StateMemberZ]; + abs_pose_var_counts[StateMemberRoll] += + pose_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + pose_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + pose_update_vec[StateMemberYaw]; + } + } + + if (twist_update_sum > 0) { + twist_var_counts[StateMemberVx] += twist_update_vec[StateMemberVx]; + twist_var_counts[StateMemberVy] += twist_update_vec[StateMemberVx]; + twist_var_counts[StateMemberVz] += twist_update_vec[StateMemberVz]; + twist_var_counts[StateMemberVroll] += + twist_update_vec[StateMemberVroll]; + twist_var_counts[StateMemberVpitch] += + twist_update_vec[StateMemberVpitch]; + twist_var_counts[StateMemberVyaw] += twist_update_vec[StateMemberVyaw]; + } + + RF_DEBUG( + "Subscribed to " << + odom_topic << " (" << odom_topic_name << ")\n\t" << + odom_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << odom_topic_name << + "_pose_rejection_threshold is " << pose_mahalanobis_thresh << + "\n\t" << odom_topic_name << "_twist_rejection_threshold is " << + twist_mahalanobis_thresh << "\n\t" << odom_topic_name << + " pose update vector is " << pose_update_vec << "\t" << + odom_topic_name << " twist update vector is " << + twist_update_vec); + } + } while (more_params); + + // Repeat for pose + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "pose" << topic_ind++; + std::string pose_topic_name = ss.str(); + std::string pose_topic; + parent_node_->declare_parameter(param_prefix + pose_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + pose_topic_name, parameter)) { + more_params = true; + pose_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + bool differential = parent_node_->declare_parameter( + param_prefix + + pose_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter( + param_prefix + + pose_topic_name + std::string("_relative"), + false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << pose_topic_name << "_differential" << " and " << pose_topic_name << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Check for pose rejection threshold + double pose_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + pose_topic_name + + std::string("_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter( + param_prefix + + pose_topic_name + + std::string("_queue_size"), 10); + + // Pull in the sensor's config, zero out values that are invalid for the + // pose type + std::vector pose_update_vec = loadUpdateConfig(pose_topic_name); + std::fill( + pose_update_vec.begin() + POSITION_V_OFFSET, + pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::fill( + pose_update_vec.begin() + POSITION_A_OFFSET, + pose_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + int pose_update_sum = + std::accumulate(pose_update_vec.begin(), pose_update_vec.end(), 0); + + if (pose_update_sum > 0) { + const CallbackData callback_data(pose_topic_name, pose_update_vec, + pose_update_sum, differential, + relative, false, pose_mahalanobis_thresh); + + std::function)> + pose_callback = + std::bind( + &UkfWrapper::poseCallback, this, std::placeholders::_1, + callback_data, world_frame_id_, base_link_frame_id_, false); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + + topic_subs_.push_back( + parent_node_->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, custom_qos, pose_callback, options)); + + if (differential) { + twist_var_counts[StateMemberVx] += pose_update_vec[StateMemberX]; + twist_var_counts[StateMemberVy] += pose_update_vec[StateMemberY]; + twist_var_counts[StateMemberVz] += pose_update_vec[StateMemberZ]; + twist_var_counts[StateMemberVroll] += + pose_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + pose_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += pose_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberX] += pose_update_vec[StateMemberX]; + abs_pose_var_counts[StateMemberY] += pose_update_vec[StateMemberY]; + abs_pose_var_counts[StateMemberZ] += pose_update_vec[StateMemberZ]; + abs_pose_var_counts[StateMemberRoll] += + pose_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + pose_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + pose_update_vec[StateMemberYaw]; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << pose_topic << " is listed as an input topic, but all pose update " + "variables are false"); + } + + RF_DEBUG( + "Subscribed to " << pose_topic << " (" << pose_topic_name << ")\n\t" << + pose_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << pose_topic_name << + "_rejection_threshold is " << pose_mahalanobis_thresh << + "\n\t" << pose_topic_name << " update vector is " << + pose_update_vec); + } + } while (more_params); + + // Repeat for gps + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "gps" << topic_ind++; + std::string gps_topic_name = ss.str(); + std::string gps_topic; + parent_node_->declare_parameter(param_prefix + gps_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + gps_topic_name, parameter)) { + more_params = true; + gps_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + bool differential = parent_node_->declare_parameter( + param_prefix + + gps_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter( + param_prefix + + gps_topic_name + std::string("_relative"), + false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << gps_topic_name << "_differential" << " and " << gps_topic_name << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Check for gps rejection threshold + double gps_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + gps_topic_name + + std::string("_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + // int queue_size = parent_node_->declare_parameter(param_prefix + + // gps_topic_name + + // std::string("_queue_size"), 10); + + // Pull in the sensor's config, zero out values that are invalid for the + // gps type + std::vector gps_update_vec = loadUpdateConfig(gps_topic_name); + std::fill( + gps_update_vec.begin() + POSITION_V_OFFSET, + gps_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::fill( + gps_update_vec.begin() + POSITION_A_OFFSET, + gps_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + int gps_update_sum = + std::accumulate(gps_update_vec.begin(), gps_update_vec.end(), 0); + + if (gps_update_sum > 0) { + const CallbackData callback_data(gps_topic_name, gps_update_vec, + gps_update_sum, differential, + relative, false, gps_mahalanobis_thresh); + + // Block Commented as it is handled in update_rt + + // std::function)> + // gps_callback = + // std::bind( + // &UkfWrapper::gpsCallback, this, std::placeholders::_1, + // callback_data, world_frame_id_, base_link_frame_id_, false); + + // auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + + // topic_subs_.push_back( + // parent_node_->create_subscription< + // geometry_msgs::msg::gpsWithCovarianceStamped>( + // gps_topic, custom_qos, gps_callback)); + + gps_callbackData_arr_.push_back(callback_data); + + if (differential) { + twist_var_counts[StateMemberVx] += gps_update_vec[StateMemberX]; + twist_var_counts[StateMemberVy] += gps_update_vec[StateMemberY]; + twist_var_counts[StateMemberVz] += gps_update_vec[StateMemberZ]; + twist_var_counts[StateMemberVroll] += + gps_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + gps_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += gps_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberX] += gps_update_vec[StateMemberX]; + abs_pose_var_counts[StateMemberY] += gps_update_vec[StateMemberY]; + abs_pose_var_counts[StateMemberZ] += gps_update_vec[StateMemberZ]; + abs_pose_var_counts[StateMemberRoll] += + gps_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + gps_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + gps_update_vec[StateMemberYaw]; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << gps_topic << " is listed as an input topic, but all pose update " + "variables are false"); + } + + RF_DEBUG( + "Subscribed to " << gps_topic << " (" << gps_topic_name << ")\n\t" << + gps_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << gps_topic_name << + "_rejection_threshold is " << gps_mahalanobis_thresh << + "\n\t" << gps_topic_name << " update vector is " << + gps_update_vec); + } + } while (more_params); + + // Repeat for twist + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "twist" << topic_ind++; + std::string twist_topic_name = ss.str(); + std::string twist_topic; + parent_node_->declare_parameter(param_prefix + twist_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + twist_topic_name, parameter)) { + more_params = true; + twist_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + // Check for twist rejection threshold + double twist_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + twist_topic_name + + std::string("_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter( + param_prefix + + twist_topic_name + + std::string("_queue_size"), 10); + + // Pull in the sensor's config, zero out values that are invalid for the + // twist type + std::vector twist_update_vec = loadUpdateConfig(twist_topic_name); + std::fill( + twist_update_vec.begin() + POSITION_OFFSET, + twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + + int twist_update_sum = + std::accumulate(twist_update_vec.begin(), twist_update_vec.end(), 0); + + if (twist_update_sum > 0) { + const CallbackData callback_data(twist_topic_name, twist_update_vec, + twist_update_sum, false, false, false, + twist_mahalanobis_thresh); + + std::function)> + twist_callback = std::bind( + &UkfWrapper::twistCallback, this, + std::placeholders::_1, callback_data, + base_link_frame_id_); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + topic_subs_.push_back( + parent_node_->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, custom_qos, twist_callback, options)); + + twist_var_counts[StateMemberVx] += twist_update_vec[StateMemberVx]; + twist_var_counts[StateMemberVy] += twist_update_vec[StateMemberVy]; + twist_var_counts[StateMemberVz] += twist_update_vec[StateMemberVz]; + twist_var_counts[StateMemberVroll] += + twist_update_vec[StateMemberVroll]; + twist_var_counts[StateMemberVpitch] += + twist_update_vec[StateMemberVpitch]; + twist_var_counts[StateMemberVyaw] += twist_update_vec[StateMemberVyaw]; + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << twist_topic << " is listed as an input topic, but all twist update " + "variables are false"); + } + + RF_DEBUG( + "Subscribed to " << twist_topic << " (" << twist_topic_name << ")\n\t" << + twist_topic_name << "_rejection_threshold is " << twist_mahalanobis_thresh << + "\n\t" << twist_topic_name << " update vector is " << twist_update_vec); + } + } while (more_params); + + // Repeat for IMU + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "imu" << topic_ind++; + std::string imu_topic_name = ss.str(); + std::string imu_topic; + parent_node_->declare_parameter(param_prefix + imu_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + imu_topic_name, parameter)) { + more_params = true; + imu_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + bool differential = parent_node_->declare_parameter( + param_prefix + + imu_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter( + param_prefix + imu_topic_name + + std::string("_relative"), false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << imu_topic_name << "_differential" << " and " << imu_topic_name << "_relative " + "were set to true. Using differential mode."); + + relative = false; + } + + // Check for pose rejection threshold + double pose_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + imu_topic_name + + std::string("_pose_rejection_threshold"), + std::numeric_limits::max()); + + // Check for angular velocity rejection threshold + std::string imu_twist_rejection_name = + imu_topic_name + std::string("_twist_rejection_threshold"); + double twist_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + imu_twist_rejection_name, + std::numeric_limits::max()); + + // Check for acceleration rejection threshold + double accel_mahalanobis_thresh = parent_node_->declare_parameter( + param_prefix + + imu_topic_name + + std::string("_linear_acceleration_rejection_threshold"), + std::numeric_limits::max()); + + bool remove_grav_acc = parent_node_->declare_parameter( + param_prefix + + imu_topic_name + + "_remove_gravitational_acceleration", + false); + remove_gravitational_acceleration_[imu_topic_name + "_acceleration"] = + remove_grav_acc; + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter( + param_prefix + + imu_topic_name + + std::string("_queue_size"), 10); + + // Now pull in its boolean update vector configuration and differential + // update configuration (as this contains pose information) + std::vector update_vec = loadUpdateConfig(imu_topic_name); + + // sanity checks for update config settings + std::vector position_update_vec(update_vec.begin() + POSITION_OFFSET, + update_vec.begin() + POSITION_OFFSET + POSITION_SIZE); + int position_update_sum = std::accumulate( + position_update_vec.begin(), + position_update_vec.end(), 0); + if (position_update_sum > 0) { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "Warning: Some position entries in parameter " << imu_topic_name << "_config are set to " + "true, but the sensor_msgs/Imu message contains no positional data"); + } + std::vector linear_velocity_update_vec( + update_vec.begin() + POSITION_V_OFFSET, + update_vec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE); + int linear_velocity_update_sum = std::accumulate( + linear_velocity_update_vec.begin(), linear_velocity_update_vec.end(), + 0); + if (linear_velocity_update_sum > 0) { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "Warning: Some position entries in parameter " << imu_topic_name << "_config are set to " + "true, but the sensor_msgs/Imu message contains no linear velocity data"); + } + + std::vector pose_update_vec = update_vec; + // IMU message contains no information about position, filter everything + // except orientation + std::fill( + pose_update_vec.begin() + POSITION_OFFSET, + pose_update_vec.begin() + POSITION_OFFSET + POSITION_SIZE, 0); + std::fill( + pose_update_vec.begin() + POSITION_V_OFFSET, + pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::fill( + pose_update_vec.begin() + POSITION_A_OFFSET, + pose_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, 0); + + std::vector twist_update_vec = update_vec; + // IMU message contains no information about linear speeds, filter + // everything except angular velocity + std::fill( + twist_update_vec.begin() + POSITION_OFFSET, + twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + std::fill( + twist_update_vec.begin() + POSITION_V_OFFSET, + twist_update_vec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE, 0); + std::fill( + twist_update_vec.begin() + POSITION_A_OFFSET, + twist_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, 0); + + std::vector accel_update_vec = update_vec; + std::fill( + accel_update_vec.begin() + POSITION_OFFSET, + accel_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + std::fill( + accel_update_vec.begin() + POSITION_V_OFFSET, + accel_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + + int pose_update_sum = + std::accumulate(pose_update_vec.begin(), pose_update_vec.end(), 0); + int twist_update_sum = + std::accumulate(twist_update_vec.begin(), twist_update_vec.end(), 0); + int accelUpdateSum = + std::accumulate(accel_update_vec.begin(), accel_update_vec.end(), 0); + + // Check if we're using control input for any of the acceleration + // variables; turn off if so + if (control_update_vector[ControlMemberVx] && + static_cast(accel_update_vec[StateMemberAx])) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "X acceleration is being measured from IMU; X velocity control input is disabled"); + control_update_vector[ControlMemberVx] = 0; + } + if (control_update_vector[ControlMemberVy] && + static_cast(accel_update_vec[StateMemberAy])) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Y acceleration is being measured from IMU; Y velocity control input is disabled"); + control_update_vector[ControlMemberVy] = 0; + } + if (control_update_vector[ControlMemberVz] && + static_cast(accel_update_vec[StateMemberAz])) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Z acceleration is being measured from IMU; Z velocity control input is disabled"); + control_update_vector[ControlMemberVz] = 0; + } + + if (pose_update_sum + twist_update_sum + accelUpdateSum > 0) { + const CallbackData pose_callback_data( + imu_topic_name + "_pose", pose_update_vec, pose_update_sum, + differential, relative, false, pose_mahalanobis_thresh); + const CallbackData twist_callback_data( + imu_topic_name + "_twist", twist_update_vec, twist_update_sum, + differential, relative, false, twist_mahalanobis_thresh); + const CallbackData accel_callback_data( + imu_topic_name + "_acceleration", accel_update_vec, accelUpdateSum, + differential, relative, false, accel_mahalanobis_thresh); + + std::function)> + imu_callback = + std::bind( + &UkfWrapper::imuCallback, this, std::placeholders::_1, + imu_topic_name, pose_callback_data, + twist_callback_data, accel_callback_data); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + topic_subs_.push_back( + parent_node_->create_subscription( + imu_topic, custom_qos, imu_callback, options)); + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << imu_topic << " is listed as an input topic, but all its update variables " + "are false"); + } + + if (pose_update_sum > 0) { + if (differential) { + twist_var_counts[StateMemberVroll] += + pose_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + pose_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += pose_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberRoll] += + pose_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + pose_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + pose_update_vec[StateMemberYaw]; + } + } + + if (twist_update_sum > 0) { + twist_var_counts[StateMemberVroll] += + twist_update_vec[StateMemberVroll]; + twist_var_counts[StateMemberVpitch] += + twist_update_vec[StateMemberVpitch]; + twist_var_counts[StateMemberVyaw] += twist_update_vec[StateMemberVyaw]; + } + + RF_DEBUG( + "Subscribed to " << + imu_topic << " (" << imu_topic_name << ")\n\t" << + imu_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << imu_topic_name << + "_pose_rejection_threshold is " << pose_mahalanobis_thresh << + "\n\t" << imu_topic_name << "_twist_rejection_threshold is " << + twist_mahalanobis_thresh << "\n\t" << imu_topic_name << + "_linear_acceleration_rejection_threshold is " << + accel_mahalanobis_thresh << "\n\t" << imu_topic_name << + "_remove_gravitational_acceleration is " << + (remove_grav_acc ? "true" : "false") << "\n\t" << + imu_topic_name << " pose update vector is " << pose_update_vec << + "\t" << imu_topic_name << " twist update vector is " << + twist_update_vec << "\t" << imu_topic_name << + " acceleration update vector is " << accel_update_vec); + } + } while (more_params); + + // Now that we've checked if IMU linear acceleration is being used, we can + // determine our final control parameters + if (use_control_ && std::accumulate( + control_update_vector.begin(), + control_update_vector.end(), 0) == 0) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but control_config has only false values. No control term will " + "be used."); + use_control_ = false; + } + + // If we're using control, set the parameters and create the necessary + // subscribers + if (use_control_) { + latest_control_.resize(TWIST_SIZE); + latest_control_.setZero(); + + filter_.setControlParams( + control_update_vector, + rclcpp::Duration::from_seconds(control_timeout), + acceleration_limits, acceleration_gains, deceleration_limits, + deceleration_gains); + + // Select between TwistStamped or Twist control input + if (stamped_control_) { + stamped_control_sub_ = parent_node_->create_subscription( + "cmd_vel", rclcpp::QoS(1), + std::bind(&UkfWrapper::controlStampedCallback, this, std::placeholders::_1), options); + } else { + control_sub_ = parent_node_->create_subscription( + "cmd_vel", rclcpp::QoS(1), + std::bind(&UkfWrapper::controlCallback, this, std::placeholders::_1), options); + } + } + + /* Warn users about: + * 1. Multiple non-differential input sources + * 2. No absolute *or* velocity measurements for pose variables + */ + if (print_diagnostics_) { + for (int state_var = StateMemberX; state_var <= StateMemberYaw; ++state_var) { + if (abs_pose_var_counts[static_cast(state_var)] > 1) { + std::stringstream stream; + stream << abs_pose_var_counts[static_cast(state_var - + POSITION_OFFSET)] << " absolute pose inputs detected for " << + state_variable_names_[state_var] << + ". This may result in oscillations. Please ensure that your" + "variances for each measured variable are set appropriately."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + state_variable_names_[state_var] + "_configuration", + stream.str(), true); + } else if (abs_pose_var_counts[static_cast(state_var)] == 0) { + if ((static_cast(state_var) == StateMemberX && + twist_var_counts[static_cast(StateMemberVx)] == 0) || + (static_cast(state_var) == StateMemberY && + twist_var_counts[static_cast(StateMemberVy)] == 0) || + (static_cast(state_var) == StateMemberZ && + twist_var_counts[static_cast(StateMemberVz)] == 0 && + two_d_mode_ == false) || + (static_cast(state_var) == StateMemberRoll && + twist_var_counts[static_cast(StateMemberVroll)] == 0 && + two_d_mode_ == false) || (static_cast(state_var) == + StateMemberPitch && + twist_var_counts[static_cast(StateMemberVpitch)] == 0 && + two_d_mode_ == false) || (static_cast(state_var) == + StateMemberYaw && twist_var_counts[static_cast(StateMemberVyaw)] == + 0)) + { + std::stringstream stream; + stream << "Neither " << state_variable_names_[state_var] << " nor its " + "velocity is being measured. This will result in unbounded" + "error growth and erratic filter behavior."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + state_variable_names_[state_var] + "_configuration", + stream.str(), true); + } + } + } + } + + auto load_covariance = [this](const std::string & parameter, Eigen::MatrixXd & covariance) + { + covariance.setZero(); + std::vector covar_flat; + + parent_node_->declare_parameter(parameter, rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(parameter, covar_flat)) { + if (covar_flat.size() == STATE_SIZE) { + RCLCPP_INFO_STREAM( + parent_node_->get_logger(), "Detected a " << parameter << " parameter with " + "length " << STATE_SIZE << ". Assuming diagonal values specified."); + covariance.diagonal() = Eigen::VectorXd::Map(covar_flat.data(), STATE_SIZE); + } else if (covar_flat.size() == STATE_SIZE * STATE_SIZE) { + covariance = Eigen::MatrixXd::Map(covar_flat.data(), STATE_SIZE, STATE_SIZE); + } else { + std::string error = "Invalid " + parameter + " specified. Expected a length of " + + std::to_string(STATE_SIZE) + " or " + std::to_string(STATE_SIZE * STATE_SIZE) + + ", received length " + std::to_string(covar_flat.size()); + RCLCPP_FATAL_STREAM(parent_node_->get_logger(), error); + throw std::invalid_argument(error); + } + return true; + } + return false; + }; + + if (load_covariance(param_prefix + "process_noise_covariance", process_noise_covariance_)) { + RF_DEBUG("Process noise covariance is:\n" << process_noise_covariance_ << "\n"); + filter_.setProcessNoiseCovariance(process_noise_covariance_); + } + + if (load_covariance( + param_prefix + "initial_estimate_covariance", + initial_estimate_error_covariance_)) + { + RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); + filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); + } +} + +void UkfWrapper::odometryCallback( + const nav_msgs::msg::Odometry::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data) +{ + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << + "The " << topic_name << + " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad " + "timestamp. (message time: " << + filter_utilities::toSec(msg->header.stamp) << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + RF_DEBUG( + "Received message that preceded the most recent pose reset. " + "Ignoring..."); + + return; + } + + RF_DEBUG( + "------ UkfWrapper::odometryCallback (" << + topic_name << ") ------\n") // << "Odometry message:\n" << *msg); + + if (pose_callback_data.update_sum_ > 0) { + // Grab the pose portion of the message and pass it to the poseCallback + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pos_ptr = + std::make_shared(); + pos_ptr->header = msg->header; + pos_ptr->pose = msg->pose; // Entire pose object, also copies covariance + + + if (pose_callback_data.pose_use_child_frame_) { + poseCallback(pos_ptr, pose_callback_data, world_frame_id_, msg->child_frame_id, false); + } else { + poseCallback(pos_ptr, pose_callback_data, world_frame_id_, base_link_frame_id_, false); + } + } + + if (twist_callback_data.update_sum_ > 0) { + // Grab the twist portion of the message and pass it to the twistCallback + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist_ptr = + std::make_shared(); + twist_ptr->header = msg->header; + twist_ptr->header.frame_id = msg->child_frame_id; + twist_ptr->twist = + msg->twist; // Entire twist object, also copies covariance + + twistCallback(twist_ptr, twist_callback_data, base_link_frame_id_); + } + + RF_DEBUG( + "\n----- /UkfWrapper::odometryCallback (" << topic_name << + ") ------\n"); +} + +void UkfWrapper::poseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame, + const std::string & pose_source_frame, const bool imu_data) +{ + const std::string & topic_name = callback_data.topic_name_; + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << + "The " << topic_name << + " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad " + "timestamp. (message time: " << + filter_utilities::toSec(msg->header.stamp) << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + return; + } + + RF_DEBUG( + "------ UkfWrapper::poseCallback (" << topic_name << ") ------\n" + "Pose message:\n" << geometry_msgs::msg::to_yaml(*msg)); + + // Put the initial value in the lastMessagTimes_ for this variable if it's + // empty + if (last_message_times_.count(topic_name) == 0) { + last_message_times_.insert( + std::pair(topic_name, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (last_message_times_[topic_name] <= msg->header.stamp) { + RF_DEBUG( + "Update vector for " << topic_name << " is:\n" << + callback_data.update_vector_); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurement_covariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector update_vector_corrected = callback_data.update_vector_; + + // Prepare the pose data for inclusion in the filter + if (preparePose( + msg, topic_name, target_frame, pose_source_frame, callback_data.differential_, + callback_data.relative_, imu_data, update_vector_corrected, + measurement, measurement_covariance)) + { + // Store the measurement. Add a "pose" suffix so we know what kind of + // measurement we're dealing with when we debug the core filter logic. + enqueueMeasurement( + topic_name, measurement, measurement_covariance, + update_vector_corrected, + callback_data.rejection_threshold_, msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topic_name << "\n"); + } else { + RF_DEBUG("Did *not* enqueue measurement for " << topic_name << "\n"); + } + + last_message_times_[topic_name] = msg->header.stamp; + + RF_DEBUG( + "Last message time for " << + topic_name << " is now " << + filter_utilities::toSec(last_message_times_[topic_name]) << + "\n"); + } else { + // else if (reset_on_time_jump_ && rclcpp::Time::isSimTime()) + //{ + // reset(); + // } + + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp before that of " + "the previous message received," << " this message will be ignored. This may " + "indicate a bad timestamp. (message time: " << msg->header.stamp.nanosec << + ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + + RF_DEBUG( + "Message is too old. Last message time for " << topic_name << " is " << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << filter_utilities::toSec(msg->header.stamp) << + ".\n"); + } + + RF_DEBUG("\n----- /UkfWrapper::poseCallback (" << topic_name << ") ------\n"); +} + +void UkfWrapper::initialize() +{ + if (!parent_node_->get_clock()->started()) { + RCLCPP_INFO(parent_node_->get_logger(), "Waiting for clock to start..."); + parent_node_->get_clock()->wait_until_started(); + } + + angular_acceleration_.setZero(); + angular_acceleration_cov_.setIdentity(); + angular_acceleration_cov_ *= 1e-6; + + last_state_twist_rot_.setZero(); + + diagnostic_updater_ = std::make_unique( + parent_node_); + diagnostic_updater_->setHardwareID("none"); + + world_transform_broadcaster_ = std::make_shared( + parent_node_); + + loadParams(); + + if (print_diagnostics_) { + diagnostic_updater_->add( + "Filter diagnostic updater", this, + &UkfWrapper::aggregateDiagnostics); + } + + // Set up the frequency diagnostic + min_frequency_ = frequency_ - 2; + max_frequency_ = frequency_ + 2; + freq_diag_ = + std::make_unique( + "odometry/filtered", + *diagnostic_updater_, + diagnostic_updater::FrequencyStatusParam( + &min_frequency_, + &max_frequency_, 0.1, 10)); + + last_diag_time_ = parent_node_->now(); + last_diff_time_ = parent_node_->now().seconds(); + + // Clear out the transforms + world_base_link_trans_msg_.transform = + tf2::toMsg(tf2::Transform::getIdentity()); + + // Position publisher + rclcpp::PublisherOptions publisher_options; + publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } + position_pub_ = + parent_node_->create_publisher( + mode_prefix + "odometry/filtered", rclcpp::QoS(10), publisher_options); + + // Optional acceleration publisher + if (publish_acceleration_) { + accel_pub_ = + parent_node_->create_publisher( + mode_prefix + "accel/filtered", rclcpp::QoS(10), publisher_options); + } + + // Block commented as it is handled in the update_rt + + // const std::chrono::duration timespan{1.0 / frequency_}; + // timer_ = rclcpp::GenericTimer::make_shared( + // parent_node_->get_clock(), std::chrono::duration_cast(timespan), + // std::bind(&UkfWrapper::periodicUpdate, this), parent_node_->get_node_base_interface()->get_context()); + // parent_node_->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void UkfWrapper::periodicUpdate() +{ + // Wait for the filter to be enabled + if (!enabled_) { + RCLCPP_INFO_ONCE( + parent_node_->get_logger(), + "Filter is disabled. To enable it call the %s service", + enable_filter_srv_->get_service_name()); + return; + } + + rclcpp::Time cur_time = parent_node_->now(); + + if (toggled_on_) { + // Now we'll integrate any measurements we've received if requested, + // and update angular acceleration. + integrateMeasurements(cur_time); + differentiateMeasurements(cur_time); + } else { + // Clear out measurements since we're not currently processing new entries + clearMeasurementQueue(); + + // Reset last measurement time so we don't get a large time delta on toggle + if (filter_.getInitializedStatus()) { + filter_.setLastMeasurementTime(parent_node_->now()); + } + } + + // Get latest state and publish it + auto filtered_position = std::make_unique(); + + bool corrected_data = false; + + if (getFilteredOdometryMessage(filtered_position.get())) { + world_base_link_trans_msg_.header.stamp = + static_cast(filtered_position->header.stamp) + tf_time_offset_; + world_base_link_trans_msg_.header.frame_id = + filtered_position->header.frame_id; + world_base_link_trans_msg_.child_frame_id = + filtered_position->child_frame_id; + + world_base_link_trans_msg_.transform.translation.x = + filtered_position->pose.pose.position.x; + world_base_link_trans_msg_.transform.translation.y = + filtered_position->pose.pose.position.y; + world_base_link_trans_msg_.transform.translation.z = + filtered_position->pose.pose.position.z; + world_base_link_trans_msg_.transform.rotation = + filtered_position->pose.pose.orientation; + + // The filtered_position is the message containing the state and covariances: + // nav_msgs Odometry + if (!validateFilterOutput(filtered_position.get())) { + RCLCPP_ERROR( + parent_node_->get_logger(), + "Critical Error, NaNs were detected in the output state of the filter. " + "This was likely due to poorly coniditioned process, noise, or sensor " + "covariances."); + } + + // If we're trying to publish with the same time stamp, it means that we had a measurement get + // inserted into the filter history, and our state estimate was updated after it was already + // published. As of ROS Noetic, TF2 will issue warnings whenever this occurs, so we make this + // behavior optional. Just for safety, we also check for the condition where the last published + // stamp is *later* than this stamp. This should never happen, but we should handle the case + // anyway. + corrected_data = (!permit_corrected_publication_ && + last_published_stamp_ >= filtered_position->header.stamp); + + // If the world_frame_id_ is the odom_frame_id_ frame, then we can just + // send the transform. If the world_frame_id_ is the map_frame_id_ frame, + // we'll have some work to do. + if (publish_transform_ && !corrected_data) { + if (filtered_position->header.frame_id == odom_frame_id_) { + world_transform_broadcaster_->sendTransform(world_base_link_trans_msg_); + } else if (filtered_position->header.frame_id == map_frame_id_) { + try { + tf2::Transform world_base_link_trans; + tf2::fromMsg( + world_base_link_trans_msg_.transform, + world_base_link_trans); + + tf2::Transform base_link_odom_trans; + tf2::fromMsg( + tf_buffer_ + ->lookupTransform( + base_link_frame_id_, + odom_frame_id_, + tf2::TimePointZero) + .transform, + base_link_odom_trans); + + /* + * First, see these two references: + * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform + * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction + * We have a transform from map_frame_id_->base_link_frame_id_, but + * it would actually transform a given pose from + * base_link_frame_id_->map_frame_id_. We then used lookupTransform, + * whose first two arguments are target frame and source frame, to + * get a transform from base_link_frame_id_->odom_frame_id_. + * However, this transform would actually transform data from + * odom_frame_id_->base_link_frame_id_. Now imagine that we have a + * position in the map_frame_id_ frame. First, we multiply it by the + * inverse of the map_frame_id_->baseLinkFrameId, which will + * transform that data from map_frame_id_ to base_link_frame_id_. + * Now we want to go from base_link_frame_id_->odom_frame_id_, but + * the transform we have takes data from + * odom_frame_id_->base_link_frame_id_, so we need its inverse as + * well. We have now transformed our data from map_frame_id_ to + * odom_frame_id_. However, if we want other users to be able to do + * the same, we need to broadcast the inverse of that entire + * transform. + */ + tf2::Transform map_odom_trans; + map_odom_trans.mult(world_base_link_trans, base_link_odom_trans); + + geometry_msgs::msg::TransformStamped map_odom_trans_msg; + map_odom_trans_msg.transform = tf2::toMsg(map_odom_trans); + map_odom_trans_msg.header.stamp = + static_cast(filtered_position->header.stamp) + tf_time_offset_; + map_odom_trans_msg.header.frame_id = map_frame_id_; + map_odom_trans_msg.child_frame_id = odom_frame_id_; + + world_transform_broadcaster_->sendTransform(map_odom_trans_msg); + } catch (...) { + RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE( + parent_node_->get_logger(), + *parent_node_->get_clock(), + 5000, + "Could not obtain transform from " << odom_frame_id_ << "->" << base_link_frame_id_); + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Odometry message frame_id was " << filtered_position->header.frame_id << + ", expected " << map_frame_id_ << " or " << odom_frame_id_); + } + } + + // Retain the last published stamp so we can detect repeated transforms in future cycles + last_published_stamp_ = filtered_position->header.stamp; + + // Fire off the position and the transform + if (!corrected_data) { + position_pub_->publish(std::move(filtered_position)); + } + + if (print_diagnostics_) { + freq_diag_->tick(); + } + } + + // Publish the acceleration if desired and filter is initialized + auto filtered_acceleration = std::make_unique(); + if (!corrected_data && publish_acceleration_ && + getFilteredAccelMessage(filtered_acceleration.get())) + { + accel_pub_->publish(std::move(filtered_acceleration)); + } + + /* Diagnostics can behave strangely when playing back from bag + * files and using simulated time, so we have to check for + * time suddenly moving backwards as well as the standard + * timeout criterion before publishing. */ + + double diag_duration = (cur_time - last_diag_time_).nanoseconds(); + if (print_diagnostics_ && + (diag_duration >= diagnostic_updater_->getPeriod().nanoseconds() || + diag_duration < 0.0)) + { + diagnostic_updater_->force_update(); + last_diag_time_ = cur_time; + } + + // Clear out expired history data + if (smooth_lagged_data_) { + clearExpiredHistory(filter_.getLastMeasurementTime() - history_length_); + } + + // Warn the user if the update took too long + const double loop_elapsed = (parent_node_->now() - cur_time).seconds(); + if (loop_elapsed > 1. / frequency_) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Failed to meet update rate! Took " << std::setprecision(20) << loop_elapsed << "seconds. " + "Try decreasing the rate, limiting sensor output frequency, or limiting the number of " + "sensors."); + } +} + +void UkfWrapper::setPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + RF_DEBUG( + "------ UkfWrapper::setPoseCallback ------\nPose message:\n" << + geometry_msgs::msg::to_yaml(*msg)); + + RCLCPP_INFO_STREAM( + parent_node_->get_logger(), + "Received set_pose request with value\n" << geometry_msgs::msg::to_yaml(*msg)); + + std::string topic_name("set_pose"); + + // Get rid of any initial poses (pretend we've never had a measurement) + initial_measurements_.clear(); + previous_measurements_.clear(); + previous_measurement_covariances_.clear(); + + clearMeasurementQueue(); + + filter_state_history_.clear(); + measurement_history_.clear(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + last_set_pose_time_ = msg->header.stamp; + + // Set the state vector to the reported pose + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + std::vector update_vector(STATE_SIZE, true); + + // We only measure pose variables, so initialize the vector to 0 + measurement.setZero(); + + // Set this to the identity and let the message reset it + measurement_covariance.setIdentity(); + measurement_covariance *= 1e-6; + + // Prepare the pose data (really just using this to transform it into the + // target frame). Twist data is going to get zeroed out. + // Since pose messages do not provide a child_frame_id, it defaults to baseLinkFrameId_ + preparePose( + msg, topic_name, world_frame_id_, base_link_frame_id_, false, false, false, + update_vector, measurement, measurement_covariance); + + // For the state + const auto now = parent_node_->now(); + + // Deterministically set the internal state to the requested pose. + // This ensures a /set_pose (or /initialpose) immediately changes outputs. + filter_.setState(measurement); + filter_.setEstimateErrorCovariance(measurement_covariance); + filter_.setLastMeasurementTime(now); + + // If the filter was not initialized yet, feed an equivalent measurement + // through the filter to flip its initialized flag. + if (!filter_.getInitializedStatus()) { + Measurement pose_measurement; + pose_measurement.topic_name_ = topic_name; + pose_measurement.measurement_ = measurement; + pose_measurement.covariance_ = measurement_covariance; + pose_measurement.update_vector_ = update_vector; + pose_measurement.time_ = now; + pose_measurement.mahalanobis_thresh_ = std::numeric_limits::max(); + pose_measurement.latest_control_ = latest_control_; + pose_measurement.latest_control_time_ = latest_control_time_; + + filter_.processMeasurement(pose_measurement); + } + + RF_DEBUG("\n------ /UkfWrapper::setPoseCallback ------\n"); +} + +bool UkfWrapper::setPoseSrvCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr/*response*/) +{ + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg = + std::make_shared( + request->pose); + setPoseCallback(msg); + + return true; +} + +bool UkfWrapper::enableFilterSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr) +{ + RF_DEBUG( + "\n[" << parent_node_->get_name() << ":]" << + " ------ /UkfWrapper::enableFilterSrvCallback ------\n"); + if (enabled_) { + RCLCPP_WARN( + parent_node_->get_logger(), + "[%s:] Asking for enabling filter service, " + "but the filter was already enabled! Use param disabled_at_startup.", + parent_node_->get_name()); + } else { + RCLCPP_INFO( + parent_node_->get_logger(), + "[%s:] Enabling filter...", + parent_node_->get_name()); + enabled_ = true; + } + + return true; +} + +void UkfWrapper::twistCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame) +{ + const std::string & topic_name = callback_data.topic_name_; + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << + "The " << topic_name << + " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad " + "timestamp. (message time: " << + filter_utilities::toSec(msg->header.stamp) << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + return; + } + + RF_DEBUG( + "------ UkfWrapper::twistCallback (" << topic_name << ") ------\n" + "Twist message:\n" << geometry_msgs::msg::to_yaml(*msg)); + + if (last_message_times_.count(topic_name) == 0) { + last_message_times_.insert( + std::pair(topic_name, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (last_message_times_[topic_name] <= msg->header.stamp) { + RF_DEBUG( + "Update vector for " << topic_name << " is:\n" << + callback_data.update_vector_); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurement_covariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector update_vector_corrected = callback_data.update_vector_; + + // Prepare the twist data for inclusion in the filter + if (prepareTwist( + msg, topic_name, target_frame, update_vector_corrected, + measurement, measurement_covariance)) + { + // Store the measurement. Add a "twist" suffix so we know what kind of + // measurement we're dealing with when we debug the core filter logic. + enqueueMeasurement( + topic_name, measurement, measurement_covariance, + update_vector_corrected, + callback_data.rejection_threshold_, msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topic_name << "_twist\n"); + } else { + RF_DEBUG( + "Did *not* enqueue measurement for " << topic_name << + "_twist\n"); + } + + last_message_times_[topic_name] = msg->header.stamp; + + RF_DEBUG( + "Last message time for " << + topic_name << " is now " << + filter_utilities::toSec(last_message_times_[topic_name]) << + "\n"); + } else { + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp before that of " + "the previous message received," << " this message will be ignored. This may " + "indicate a bad timestamp. (message time: " << msg->header.stamp.nanosec << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + + RF_DEBUG( + "Message is too old. Last message time for " << topic_name << " is" << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << filter_utilities::toSec(msg->header.stamp) << + ".\n"); + } + + RF_DEBUG("\n----- /UkfWrapper::twistCallback (" << topic_name << ") ------\n"); +} + +void UkfWrapper::addDiagnostic( + const int errLevel, + const std::string & topicAndClass, + const std::string & message, + const bool staticDiag) +{ + if (staticDiag) { + static_diagnostics_[topicAndClass] = message; + static_diag_error_level_ = std::max(static_diag_error_level_, errLevel); + } else { + dynamic_diagnostics_[topicAndClass] = message; + dynamic_diag_error_level_ = std::max(dynamic_diag_error_level_, errLevel); + } +} + +void UkfWrapper::aggregateDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper & wrapper) +{ + wrapper.clear(); + wrapper.clearSummary(); + + int maxErrLevel = std::max(static_diag_error_level_, dynamic_diag_error_level_); + + // Report the overall status + switch (maxErrLevel) { + case diagnostic_msgs::msg::DiagnosticStatus::ERROR: + wrapper.summary( + maxErrLevel, + "Erroneous data or settings detected for a " + "robot_localization state estimation parent_node_->"); + break; + case + diagnostic_msgs::msg::DiagnosticStatus::WARN: wrapper.summary( + maxErrLevel, + "Potentially erroneous data or settings detected for " + "a robot_localization state estimation parent_node_->"); + break; + case diagnostic_msgs::msg::DiagnosticStatus::STALE: + wrapper.summary( + maxErrLevel, + "The state of the robot_localization state estimation " + "node is stale."); + break; + case diagnostic_msgs::msg::DiagnosticStatus::OK: + wrapper.summary( + maxErrLevel, + "The robot_localization state estimation node appears to " + "be functioning properly."); + break; + default: + break; + } + + // Aggregate all the static messages + for (std::map::iterator diagIt = + static_diagnostics_.begin(); diagIt != static_diagnostics_.end(); + ++diagIt) + { + wrapper.add(diagIt->first, diagIt->second); + } + + // Aggregate all the dynamic messages, then clear them + for (std::map::iterator diagIt = + dynamic_diagnostics_.begin(); diagIt != dynamic_diagnostics_.end(); + ++diagIt) + { + wrapper.add(diagIt->first, diagIt->second); + } + dynamic_diagnostics_.clear(); + + // Reset the warning level for the dynamic diagnostic messages + dynamic_diag_error_level_ = diagnostic_msgs::msg::DiagnosticStatus::OK; +} + +void UkfWrapper::copyCovariance( + const double * arr, Eigen::MatrixXd & covariance, + const std::string & topic_name, + const std::vector & update_vector, + const size_t offset, const size_t dimension) +{ + for (size_t i = 0; i < dimension; i++) { + for (size_t j = 0; j < dimension; j++) { + covariance(i, j) = arr[dimension * i + j]; + + if (print_diagnostics_) { + std::string iVar = state_variable_names_[offset + i]; + + if (covariance(i, j) > 1e3 && (update_vector[offset + i] || + update_vector[offset + j])) + { + std::string jVar = state_variable_names_[offset + j]; + + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << (i == j ? iVar + " variance" : iVar + " and " + + jVar + " covariance") << + ", the value is extremely large (" << covariance(i, j) << "), but " + "the update vector for " << (i == j ? iVar : iVar + " and/or " + jVar) << + "is set to true. This may produce undesirable results."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_covariance", stream.str(), false); + } else if (update_vector[i] && i == j && covariance(i, j) == 0) { + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << iVar << " variance, was zero. This" + "will be replaced with a small value to maintain filter stability, " + "but should be corrected at the message origin parent_node_->"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_covariance", stream.str(), false); + } else if (update_vector[i] && i == j && covariance(i, j) < 0) { + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << iVar << " variance, was" + "negative. This will be replaced with a small positive value to maintain" + "filter stability, but should be corrected at the message origin parent_node_->"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_covariance", stream.str(), false); + } + } + } + } +} + +void UkfWrapper::copyCovariance( + const Eigen::MatrixXd & covariance, double * arr, + const size_t dimension) +{ + for (size_t i = 0; i < dimension; i++) { + for (size_t j = 0; j < dimension; j++) { + arr[dimension * i + j] = covariance(i, j); + } + } +} + +std::vector UkfWrapper::loadUpdateConfig(const std::string & topic_name) +{ + std::vector update_vector(STATE_SIZE, 0); + std::string prefix = plugin_name_.empty() ? "" : plugin_name_ + "."; + const std::string topic_config_name = prefix + topic_name + "_config"; + + update_vector = parent_node_->declare_parameter(topic_config_name, update_vector); + + return update_vector; +} + +bool UkfWrapper::prepareAcceleration( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const std::string & target_frame, + const bool relative, + std::vector & update_vector, + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance) +{ + RF_DEBUG( + "------ UkfWrapper::prepareAcceleration (" << topic_name << + ") ------\n"); + + // 1. Get the measurement into a vector + tf2::Vector3 acc_tmp(msg->linear_acceleration.x, msg->linear_acceleration.y, + msg->linear_acceleration.z); + + // Set relevant header info + std::string msg_frame = + (msg->header.frame_id == "" ? base_link_frame_id_ : msg->header.frame_id); + + // 2. robot_localization lets users configure which variables from the sensor + // should be + // fused with the filter. This is specified at the sensor level. However, + // the data may go through transforms before being fused with the state + // estimate. In that case, we need to know which of the transformed + // variables came from the pre-transformed "approved" variables (i.e., the + // ones that had "true" in their xxx_config parameter). To do this, we + // create a pose from the original upate vector, which contains only zeros + // and ones. This pose goes through the same transforms as the measurement. + // The non-zero values that result will be used to modify the + // update_vector. + tf2::Matrix3x3 maskAcc(update_vector[StateMemberAx], 0, 0, 0, + update_vector[StateMemberAy], 0, 0, 0, + update_vector[StateMemberAz]); + + // 3. We'll need to rotate the covariance as well + Eigen::MatrixXd covariance_rotated(ACCELERATION_SIZE, ACCELERATION_SIZE); + covariance_rotated.setZero(); + + this->copyCovariance( + &(msg->linear_acceleration_covariance[0]), + covariance_rotated, topic_name, update_vector, + POSITION_A_OFFSET, ACCELERATION_SIZE); + + RF_DEBUG( + "Original measurement as tf object: " << + acc_tmp << "\nOriginal update vector:\n" << + update_vector << "\nOriginal covariance matrix:\n" << + covariance_rotated << "\n"); + + // 4. We need to transform this into the target frame (probably base_link) + // It's unlikely that we'll get a velocity measurement in another frame, but + // we have to handle the situation. + tf2::Transform target_frame_trans; + bool can_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, + target_frame_trans); + + if (can_transform) { + const Eigen::VectorXd & state = filter_.getState(); + + // Transform to correct frame, prior to removal of gravity. + tf2::Vector3 state_twist_rot( + state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + acc_tmp = target_frame_trans.getBasis() * acc_tmp + + target_frame_trans.getOrigin().cross(angular_acceleration_) - + target_frame_trans.getOrigin().cross(state_twist_rot).cross( + state_twist_rot); + + // We don't know if the user has already handled the removal + // of normal forces, so we use a parameter + if (remove_gravitational_acceleration_[topic_name]) { + tf2::Vector3 normAcc(0, 0, gravitational_acceleration_); + tf2::Transform trans; + tf2::Vector3 rotNorm; + + if (std::abs(msg->orientation_covariance[0] + 1) < 1e-9) { + // Imu message contains no orientation, so we should use orientation + // from filter state to transform and remove acceleration + tf2::Matrix3x3 stateTmp; + stateTmp.setRPY( + state(StateMemberRoll), + state(StateMemberPitch), + state(StateMemberYaw)); + + // transform state orientation to IMU frame + trans.setBasis(stateTmp * target_frame_trans.getBasis()); + rotNorm = trans.getBasis().inverse() * normAcc; + } else { + tf2::Quaternion curAttitude; + tf2::fromMsg(msg->orientation, curAttitude); + if (std::abs(curAttitude.length() - 1.0) > 0.01) { + RCLCPP_WARN_ONCE( + parent_node_->get_logger(), + "An input was not normalized, this should NOT happen, but will normalize."); + curAttitude.normalize(); + } + trans.setRotation(curAttitude); + if (!relative) { + // curAttitude is the true world-frame attitude of the sensor + rotNorm = trans.getBasis().inverse() * normAcc; + } else { + // curAttitude is relative to the initial pose of the sensor. + // Assumption: IMU sensor is rigidly attached to the base_link + // (but a static rotation is possible). + rotNorm = target_frame_trans.getBasis().inverse() * trans.getBasis().inverse() * normAcc; + } + } + acc_tmp.setX(acc_tmp.getX() - rotNorm.getX()); + acc_tmp.setY(acc_tmp.getY() - rotNorm.getY()); + acc_tmp.setZ(acc_tmp.getZ() - rotNorm.getZ()); + + RF_DEBUG( + "Orientation is " << + trans.getRotation() << "Acceleration due to gravity is " << rotNorm << + "After removing acceleration due to gravity, acceleration is " << + acc_tmp << "\n"); + } + + maskAcc = target_frame_trans.getBasis() * maskAcc; + + // Now use the mask values to determine which update vector values should be + // true + update_vector[StateMemberAx] = static_cast( + maskAcc.getRow(StateMemberAx - POSITION_A_OFFSET).length() >= 1e-6); + update_vector[StateMemberAy] = static_cast( + maskAcc.getRow(StateMemberAy - POSITION_A_OFFSET).length() >= 1e-6); + update_vector[StateMemberAz] = static_cast( + maskAcc.getRow(StateMemberAz - POSITION_A_OFFSET).length() >= 1e-6); + + RF_DEBUG( + msg->header.frame_id << + "->" << target_frame << " transform:\n" << + target_frame_trans << "\nAfter applying transform to " << + target_frame << ", update vector is:\n" << + update_vector << "\nAfter applying transform to " << + target_frame << ", measurement is:\n" << + acc_tmp << "\n"); + + // 5. Now rotate the covariance: create an augmented + // matrix that contains a 3D rotation matrix in the + // upper-left and lower-right quadrants, and zeros + // elsewhere + tf2::Matrix3x3 rot(target_frame_trans.getRotation()); + Eigen::MatrixXd rot3d(ACCELERATION_SIZE, ACCELERATION_SIZE); + rot3d.setIdentity(); + + for (size_t r_ind = 0; r_ind < ACCELERATION_SIZE; ++r_ind) { + rot3d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot3d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot3d(r_ind, 2) = rot.getRow(r_ind).getZ(); + } + + // Carry out the rotation + covariance_rotated = rot3d * covariance_rotated.eval() * rot3d.transpose(); + + RF_DEBUG("Transformed covariance is \n" << covariance_rotated << "\n"); + + // 6. Store our corrected measurement and covariance + measurement(StateMemberAx) = acc_tmp.getX(); + measurement(StateMemberAy) = acc_tmp.getY(); + measurement(StateMemberAz) = acc_tmp.getZ(); + + // Copy the covariances + measurement_covariance.block( + POSITION_A_OFFSET, POSITION_A_OFFSET, + ACCELERATION_SIZE, ACCELERATION_SIZE) = + covariance_rotated.block(0, 0, ACCELERATION_SIZE, ACCELERATION_SIZE); + + // 7. Handle 2D mode + if (two_d_mode_) { + forceTwoD(measurement, measurement_covariance, update_vector); + } + } else { + RF_DEBUG( + "Could not transform measurement into " << target_frame << + ". Ignoring...\n"); + } + + RF_DEBUG( + "\n----- /UkfWrapper::prepareAcceleration(" << topic_name << + ") ------\n"); + + return can_transform; +} + +bool UkfWrapper::preparePose( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const std::string & topic_name, const std::string & target_frame, + const std::string & source_frame, + const bool differential, const bool relative, const bool imu_data, + std::vector & update_vector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance) +{ + bool retVal = false; + + RF_DEBUG("------ UkfWrapper::preparePose (" << topic_name << ") ------\n"); + + // 1. Get the measurement into a tf-friendly transform (pose) object + tf2::Stamped pose_tmp; + + // We'll need this later for storing this measurement for differential + // integration + tf2::Transform cur_measurement; + + // Handle issues where frame_id data is not filled out properly + // @todo: verify that this is necessary still. New IMU handling may + // have rendered this obsolete. + std::string final_target_frame; + if (target_frame == "") { + if (msg->header.frame_id == "") { + // Blank target and message frames mean we can just + // use our world_frame + final_target_frame = world_frame_id_; + pose_tmp.frame_id_ = final_target_frame; + } else { + // Under the (target_frame == "") condition, + // a blank target frame means we shouldn't bother + // transforming the data + final_target_frame = msg->header.frame_id; + pose_tmp.frame_id_ = final_target_frame; + } + } else { + // Otherwise, we should use our target frame + final_target_frame = target_frame; + pose_tmp.frame_id_ = + (differential && !imu_data ? final_target_frame : msg->header.frame_id); + } + + RF_DEBUG( + "Final target frame for " << topic_name << " is " << + final_target_frame << "\n"); + + pose_tmp.stamp_ = tf2::timeFromSec( + static_cast(msg->header.stamp.sec) + + static_cast(msg->header.stamp.nanosec) / 1000000000.0); + + // Fill out the position data + pose_tmp.setOrigin( + tf2::Vector3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z)); + + tf2::Quaternion orientation; + + // Handle bad (empty) quaternions + if (msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 && + msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0) + { + orientation.setValue(0.0, 0.0, 0.0, 1.0); + + if (update_vector[StateMemberRoll] || + update_vector[StateMemberPitch] || + update_vector[StateMemberYaw]) + { + std::stringstream stream; + stream << "The " << topic_name << + " message contains an invalid orientation quaternion, " << + "but its configuration is such that orientation data is being used." + " Correcting..."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_orientation", stream.str(), false); + } + } else { + tf2::fromMsg(msg->pose.pose.orientation, orientation); + if (std::abs(orientation.length() - 1.0) > 0.01) { + RCLCPP_WARN_ONCE( + parent_node_->get_logger(), + "An input was not normalized, this should NOT happen, but will normalize."); + orientation.normalize(); + } + } + + // Fill out the orientation data + pose_tmp.setRotation(orientation); + + // 2. Get the target frame transformation + tf2::Transform target_frame_trans; + bool can_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_, final_target_frame, pose_tmp.frame_id_, + rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, + target_frame_trans); + + // handling multiple odometry origins: convert to the origin adherent to base_link. + // make pose refer to the baseLinkFrame as source + tf2::Transform source_frame_trans; + bool can_src_transform = false; + if (source_frame != base_link_frame_id_) { + can_src_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_, source_frame, base_link_frame_id_, + rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, + source_frame_trans); + } + + // 3. Make sure we can work with this data before carrying on + if (can_transform) { + /* 4. robot_localization lets users configure which variables from the + * sensor should be fused with the filter. This is specified at the sensor + * level. However, the data may go through transforms before being fused + * with the state estimate. In that case, we need to know which of the + * transformed variables came from the pre-transformed "approved" variables + * (i.e., the ones that had "true" in their xxx_config parameter). To do + * this, we construct matrices using the update vector values on the + * diagonals, pass this matrix through the rotation, and use the length of + * each row to determine the transformed update vector. The process is + * slightly different for IMUs, as the coordinate frame transform is really + * the base_link->imu_frame transform, and not a transform from some other + * world-fixed frame (even though the IMU data itself *is* reported in a + * world fixed frame). */ + tf2::Matrix3x3 mask_position(update_vector[StateMemberX], 0, 0, 0, + update_vector[StateMemberY], 0, 0, 0, + update_vector[StateMemberZ]); + + tf2::Matrix3x3 mask_orientation(update_vector[StateMemberRoll], 0, 0, 0, + update_vector[StateMemberPitch], 0, 0, 0, + update_vector[StateMemberYaw]); + + if (imu_data) { + /* We have to treat IMU orientation data differently. Even though we are + * dealing with pose data when we work with orientations, for IMUs, the + * frame_id is the frame in which the sensor is mounted, and not the + * coordinate frame of the IMU. Imagine an IMU that is mounted facing + * sideways. The pitch in the IMU frame becomes roll for the vehicle. This + * means that we need to rotate roll and pitch angles by the IMU's + * mounting yaw offset, and we must apply similar treatment to its update + * mask and covariance. + * */ + + double dummy, yaw; + target_frame_trans.getBasis().getRPY(dummy, dummy, yaw); + tf2::Matrix3x3 trans_tmp; + trans_tmp.setRPY(0.0, 0.0, yaw); + + mask_position = trans_tmp * mask_position; + mask_orientation = trans_tmp * mask_orientation; + } else { + mask_position = target_frame_trans.getBasis() * mask_position; + mask_orientation = target_frame_trans.getBasis() * mask_orientation; + } + + // Now copy the mask values back into the update vector: any row with a + // significant vector length indicates that we want to set that variable to + // true in the update vector. + update_vector[StateMemberX] = static_cast( + mask_position.getRow(StateMemberX - POSITION_OFFSET).length() >= 1e-6); + update_vector[StateMemberY] = static_cast( + mask_position.getRow(StateMemberY - POSITION_OFFSET).length() >= 1e-6); + update_vector[StateMemberZ] = static_cast( + mask_position.getRow(StateMemberZ - POSITION_OFFSET).length() >= 1e-6); + update_vector[StateMemberRoll] = static_cast( + mask_orientation.getRow(StateMemberRoll - ORIENTATION_OFFSET) + .length() >= 1e-6); + update_vector[StateMemberPitch] = static_cast( + mask_orientation.getRow(StateMemberPitch - ORIENTATION_OFFSET) + .length() >= 1e-6); + update_vector[StateMemberYaw] = static_cast( + mask_orientation.getRow(StateMemberYaw - ORIENTATION_OFFSET).length() >= + 1e-6); + + // 5a. We'll need to rotate the covariance as well. Create a container and + // copy over the covariance data + Eigen::MatrixXd covariance(POSE_SIZE, POSE_SIZE); + covariance.setZero(); + copyCovariance( + &(msg->pose.covariance[0]), covariance, topic_name, + update_vector, POSITION_OFFSET, POSE_SIZE); + + // 5b. Now rotate the covariance: create an augmented matrix that + // contains a 3D rotation matrix in the upper-left and lower-right + // quadrants, with zeros elsewhere. + tf2::Matrix3x3 rot; + Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE); + rot6d.setIdentity(); + Eigen::MatrixXd covariance_rotated; + + // Transform pose covariance due to a different pose source origin + if (can_src_transform) { + // (source_frame != base_link_frame_id_) already satisfied + rot.setRotation(source_frame_trans.getRotation()); + for (size_t r_ind = 0; r_ind < POSITION_SIZE; ++r_ind) { + // let's borrow rot6d here... + rot6d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot6d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot6d(r_ind, 2) = rot.getRow(r_ind).getZ(); + rot6d(r_ind + POSITION_SIZE, 3) = rot.getRow(r_ind).getX(); + rot6d(r_ind + POSITION_SIZE, 4) = rot.getRow(r_ind).getY(); + rot6d(r_ind + POSITION_SIZE, 5) = rot.getRow(r_ind).getZ(); + } + // since the transformation is a post-multiply + covariance = rot6d.transpose() * covariance.eval() * rot6d; + } + // return rot6d to its initial state. + rot6d.setIdentity(); + + if (imu_data) { + // Apply the same special logic to the IMU covariance rotation + double dummy, yaw; + target_frame_trans.getBasis().getRPY(dummy, dummy, yaw); + rot.setRPY(0.0, 0.0, yaw); + } else { + rot.setRotation(target_frame_trans.getRotation()); + } + + for (size_t r_ind = 0; r_ind < POSITION_SIZE; ++r_ind) { + rot6d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot6d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot6d(r_ind, 2) = rot.getRow(r_ind).getZ(); + rot6d(r_ind + POSITION_SIZE, 3) = rot.getRow(r_ind).getX(); + rot6d(r_ind + POSITION_SIZE, 4) = rot.getRow(r_ind).getY(); + rot6d(r_ind + POSITION_SIZE, 5) = rot.getRow(r_ind).getZ(); + } + + // Now carry out the rotation + covariance_rotated = rot6d * covariance * rot6d.transpose(); + + RF_DEBUG( + "After rotating into the " << final_target_frame << + " frame, covariance is \n" << + covariance_rotated << "\n"); + + /* 6a. For IMU data, the transform that we get is the transform from the + * body frame of the robot (e.g., base_link) to the mounting frame of the + * robot. It is *not* the coordinate frame in which the IMU orientation data + * is reported. If the IMU is mounted in a non-neutral orientation, we need + * to remove those offsets, and then we need to potentially "swap" roll and + * pitch. Note that this transform does NOT handle NED->ENU conversions. + * Data is assumed to be in the ENU frame when it is received. + * */ + if (imu_data) { + // First, convert the transform and measurement rotation to RPY + // @todo: There must be a way to handle this with quaternions. Need to + // look into it. + double roll_offset = 0; + double pitch_offset = 0; + double yaw_offset = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + ros_filter_utilities::quatToRPY( + target_frame_trans.getRotation(), + roll_offset, pitch_offset, yaw_offset); + ros_filter_utilities::quatToRPY(pose_tmp.getRotation(), roll, pitch, yaw); + + // 6b. Apply the offset (making sure to bound them), and throw them in a + // vector + tf2::Vector3 rpy_angles( + angles::normalize_angle(roll - roll_offset), + angles::normalize_angle(pitch - pitch_offset), + angles::normalize_angle(yaw - yaw_offset)); + + // 6c. Now we need to rotate the roll and pitch by the yaw offset value. + // Imagine a case where an IMU is mounted facing sideways. In that case + // pitch for the IMU's world frame is roll for the robot. + tf2::Matrix3x3 mat; + mat.setRPY(0.0, 0.0, yaw_offset); + rpy_angles = mat * rpy_angles; + pose_tmp.getBasis().setRPY( + rpy_angles.getX(), rpy_angles.getY(), + rpy_angles.getZ()); + + // We will use this target transformation later on, but + // we've already transformed this data as if the IMU + // were mounted neutrall on the robot, so we can just + // make the transform the identity. + target_frame_trans.setIdentity(); + } + + // 7. Two cases: if we're in differential mode, we need to generate a twist + // message. Otherwise, we just transform it to the target frame. + if (differential) { + bool success = false; + + // We're going to be playing with pose_tmp, so store it, + // as we'll need to save its current value for the next + // measurement. + cur_measurement = pose_tmp; + + // Make sure we have previous measurements to work with + if (previous_measurements_.count(topic_name) > 0 && + previous_measurement_covariances_.count(topic_name) > 0) + { + // 7a. If we are carrying out differential integration and + // we have a previous measurement for this sensor,then we + // need to apply the inverse of that measurement to this new + // measurement to produce a "delta" measurement between the two. + // Even if we're not using all of the variables from this sensor, + // we need to use the whole measurement to determine the delta + // to the new measurement + tf2::Transform prev_measurement = previous_measurements_[topic_name]; + pose_tmp.setData(prev_measurement.inverseTimes(pose_tmp)); + + RF_DEBUG( + "Previous measurement:\n" << + previous_measurements_[topic_name] << + "\nAfter removing previous measurement, measurement delta is:\n" << + pose_tmp << "\n"); + + // 7b. Now we we have a measurement delta in the frame_id of the + // message, but we want that delta to be in the target frame, so + // we need to apply the rotation of the target frame transform. + target_frame_trans.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + pose_tmp.mult(target_frame_trans, pose_tmp); + + RF_DEBUG( + "After rotating to the target frame, measurement delta is:\n" << + pose_tmp << "\n"); + + // 7c. Now use the time difference from the last message to compute + // translational and rotational velocities + double dt = filter_utilities::toSec(msg->header.stamp) - + filter_utilities::toSec(last_message_times_[topic_name]); + double xVel = pose_tmp.getOrigin().getX() / dt; + double yVel = pose_tmp.getOrigin().getY() / dt; + double zVel = pose_tmp.getOrigin().getZ() / dt; + + double rollVel = 0; + double pitchVel = 0; + double yawVel = 0; + + ros_filter_utilities::quatToRPY( + pose_tmp.getRotation(), rollVel, + pitchVel, yawVel); + rollVel /= dt; + pitchVel /= dt; + yawVel /= dt; + + RF_DEBUG( + "Previous message time was " << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << + filter_utilities::toSec(msg->header.stamp) << ", delta is " << + dt << ", velocity is (vX, vY, vZ): (" << xVel << ", " << + yVel << ", " << zVel << ")\n" << + "(vRoll, vPitch, vYaw): (" << rollVel << ", " << pitchVel << + ", " << yawVel << ")\n"); + + // 7d. Fill out the velocity data in the message + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist_ptr = + std::make_shared(); + twist_ptr->header = msg->header; + twist_ptr->header.frame_id = source_frame; + twist_ptr->twist.twist.linear.x = xVel; + twist_ptr->twist.twist.linear.y = yVel; + twist_ptr->twist.twist.linear.z = zVel; + twist_ptr->twist.twist.angular.x = rollVel; + twist_ptr->twist.twist.angular.y = pitchVel; + twist_ptr->twist.twist.angular.z = yawVel; + std::vector twist_update_vec(STATE_SIZE, false); + std::copy( + update_vector.begin() + POSITION_OFFSET, + update_vector.begin() + POSE_SIZE, + twist_update_vec.begin() + POSITION_V_OFFSET); + std::copy( + twist_update_vec.begin(), twist_update_vec.end(), + update_vector.begin()); + + // 7e. Now rotate the previous covariance for this measurement to get it + // into the target frame, and add the current measurement's rotated + // covariance to the previous measurement's rotated covariance, and + // multiply by the time delta. + Eigen::MatrixXd prev_covar_rotated = + rot6d * previous_measurement_covariances_[topic_name] * + rot6d.transpose(); + covariance_rotated = + (covariance_rotated.eval() + prev_covar_rotated) * dt; + copyCovariance( + covariance_rotated, &(twist_ptr->twist.covariance[0]), + POSE_SIZE); + + RF_DEBUG( + "Previous measurement covariance:\n" << + previous_measurement_covariances_[topic_name] << + "\nPrevious measurement covariance rotated:\n" << + prev_covar_rotated << "\nFinal twist covariance:\n" << + covariance_rotated << "\n"); + + // Now pass this on to prepareTwist, which will convert it to the + // required frame + success = prepareTwist( + twist_ptr, topic_name + "_twist", + base_link_frame_id_, update_vector, + measurement, measurement_covariance); + } + + // 7f. Update the previous measurement and measurement covariance + previous_measurements_[topic_name] = cur_measurement; + previous_measurement_covariances_[topic_name] = covariance; + + retVal = success; + } else { + // make pose refer to the baseLinkFrame as source + // can_src_transform == true => ( sourceFrame != baseLinkFrameId_ ) + if (can_src_transform) { + pose_tmp.setData(pose_tmp * source_frame_trans); + } + + // 7g. If we're in relative mode, remove the initial measurement + if (relative) { + if (initial_measurements_.count(topic_name) == 0) { + initial_measurements_.insert( + std::pair(topic_name, pose_tmp)); + } + + tf2::Transform initial_measurement = initial_measurements_[topic_name]; + pose_tmp.setData(initial_measurement.inverseTimes(pose_tmp)); + } + + // 7h. Apply the target frame transformation to the pose object. + pose_tmp.mult(target_frame_trans, pose_tmp); + pose_tmp.frame_id_ = final_target_frame; + + // 7i. Finally, copy everything into our measurement and covariance + // objects + measurement(StateMemberX) = pose_tmp.getOrigin().x(); + measurement(StateMemberY) = pose_tmp.getOrigin().y(); + measurement(StateMemberZ) = pose_tmp.getOrigin().z(); + + // The filter needs roll, pitch, and yaw values instead of quaternions + double roll, pitch, yaw; + ros_filter_utilities::quatToRPY(pose_tmp.getRotation(), roll, pitch, yaw); + measurement(StateMemberRoll) = roll; + measurement(StateMemberPitch) = pitch; + measurement(StateMemberYaw) = yaw; + + measurement_covariance.block(0, 0, POSE_SIZE, POSE_SIZE) = + covariance_rotated.block(0, 0, POSE_SIZE, POSE_SIZE); + + // 8. Handle 2D mode + if (two_d_mode_) { + forceTwoD(measurement, measurement_covariance, update_vector); + } + + retVal = true; + } + } else { + retVal = false; + + RF_DEBUG( + "Could not transform measurement into " << final_target_frame << + ". Ignoring..."); + } + + RF_DEBUG("\n----- /UkfWrapper::preparePose (" << topic_name << ") ------\n"); + + return retVal; +} + +bool UkfWrapper::prepareTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const std::string & topic_name, const std::string & target_frame, + std::vector & update_vector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance) +{ + RF_DEBUG("------ UkfWrapper::prepareTwist (" << topic_name << ") ------\n"); + + // 1. Get the measurement into two separate vector objects. + tf2::Vector3 twist_lin(msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + tf2::Vector3 meas_twist_rot(msg->twist.twist.angular.x, + msg->twist.twist.angular.y, + msg->twist.twist.angular.z); + + // 1a. This sensor may or may not measure rotational velocity. Regardless, + // if it measures linear velocity, then later on, we'll need to remove "false" + // linear velocity resulting from angular velocity and the translational + // offset of the sensor from the vehicle origin. + const Eigen::VectorXd & state = filter_.getState(); + tf2::Vector3 state_twist_rot(state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + + // Determine the frame_id of the data + std::string msg_frame = + (msg->header.frame_id == "" ? target_frame : msg->header.frame_id); + + // 2. robot_localization lets users configure which variables from the sensor + // should be + // fused with the filter. This is specified at the sensor level. However, + // the data may go through transforms before being fused with the state + // estimate. In that case, we need to know which of the transformed + // variables came from the pre-transformed "approved" variables (i.e., the + // ones that had "true" in their xxx_config parameter). To do this, we + // construct matrices using the update vector values on the diagonals, pass + // this matrix through the rotation, and use the length of each row to + // determine the transformed update vector. + tf2::Matrix3x3 maskLin(update_vector[StateMemberVx], 0, 0, 0, + update_vector[StateMemberVy], 0, 0, 0, + update_vector[StateMemberVz]); + + tf2::Matrix3x3 maskRot(update_vector[StateMemberVroll], 0, 0, 0, + update_vector[StateMemberVpitch], 0, 0, 0, + update_vector[StateMemberVyaw]); + + // 3. We'll need to rotate the covariance as well + Eigen::MatrixXd covariance_rotated(TWIST_SIZE, TWIST_SIZE); + covariance_rotated.setZero(); + + copyCovariance( + &(msg->twist.covariance[0]), covariance_rotated, topic_name, + update_vector, POSITION_V_OFFSET, TWIST_SIZE); + + RF_DEBUG( + "Original measurement as tf object:\nLinear: " << + twist_lin << "Rotational: " << meas_twist_rot << + "\nOriginal update vector:\n" << + update_vector << "\nOriginal covariance matrix:\n" << + covariance_rotated << "\n"); + + // 4. We need to transform this into the target frame (probably base_link) + tf2::Transform target_frame_trans; + bool can_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, + target_frame_trans); + + if (can_transform) { + // Transform to correct frame. Note that we can get linear velocity + // as a result of the sensor offset and rotational velocity + meas_twist_rot = target_frame_trans.getBasis() * meas_twist_rot; + twist_lin = target_frame_trans.getBasis() * twist_lin + + target_frame_trans.getOrigin().cross(state_twist_rot); + maskLin = target_frame_trans.getBasis() * maskLin; + maskRot = target_frame_trans.getBasis() * maskRot; + + // Now copy the mask values back into the update vector + update_vector[StateMemberVx] = static_cast( + maskLin.getRow(StateMemberVx - POSITION_V_OFFSET).length() >= 1e-6); + update_vector[StateMemberVy] = static_cast( + maskLin.getRow(StateMemberVy - POSITION_V_OFFSET).length() >= 1e-6); + update_vector[StateMemberVz] = static_cast( + maskLin.getRow(StateMemberVz - POSITION_V_OFFSET).length() >= 1e-6); + update_vector[StateMemberVroll] = static_cast( + maskRot.getRow(StateMemberVroll - ORIENTATION_V_OFFSET).length() >= + 1e-6); + update_vector[StateMemberVpitch] = static_cast( + maskRot.getRow(StateMemberVpitch - ORIENTATION_V_OFFSET).length() >= + 1e-6); + update_vector[StateMemberVyaw] = static_cast( + maskRot.getRow(StateMemberVyaw - ORIENTATION_V_OFFSET).length() >= + 1e-6); + + RF_DEBUG( + msg->header.frame_id << + "->" << target_frame << " transform:\n" << + target_frame_trans << "\nAfter applying transform to " << + target_frame << ", update vector is:\n" << + update_vector << "\nAfter applying transform to " << + target_frame << ", measurement is:\n" << + "Linear: " << twist_lin << "Rotational: " << meas_twist_rot << + "\n"); + + // 5. Now rotate the covariance: create an augmented + // matrix that contains a 3D rotation matrix in the + // upper-left and lower-right quadrants, and zeros + // elsewhere + tf2::Matrix3x3 rot(target_frame_trans.getRotation()); + Eigen::MatrixXd rot6d(TWIST_SIZE, TWIST_SIZE); + rot6d.setIdentity(); + + for (size_t r_ind = 0; r_ind < POSITION_SIZE; ++r_ind) { + rot6d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot6d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot6d(r_ind, 2) = rot.getRow(r_ind).getZ(); + rot6d(r_ind + POSITION_SIZE, 3) = rot.getRow(r_ind).getX(); + rot6d(r_ind + POSITION_SIZE, 4) = rot.getRow(r_ind).getY(); + rot6d(r_ind + POSITION_SIZE, 5) = rot.getRow(r_ind).getZ(); + } + + // Carry out the rotation + covariance_rotated = rot6d * covariance_rotated.eval() * rot6d.transpose(); + + RF_DEBUG("Transformed covariance is \n" << covariance_rotated << "\n"); + + // 6. Store our corrected measurement and covariance + measurement(StateMemberVx) = twist_lin.getX(); + measurement(StateMemberVy) = twist_lin.getY(); + measurement(StateMemberVz) = twist_lin.getZ(); + measurement(StateMemberVroll) = meas_twist_rot.getX(); + measurement(StateMemberVpitch) = meas_twist_rot.getY(); + measurement(StateMemberVyaw) = meas_twist_rot.getZ(); + + // Copy the covariances + measurement_covariance.block( + POSITION_V_OFFSET, POSITION_V_OFFSET, + TWIST_SIZE, TWIST_SIZE) = + covariance_rotated.block(0, 0, TWIST_SIZE, TWIST_SIZE); + + // 7. Handle 2D mode + if (two_d_mode_) { + forceTwoD(measurement, measurement_covariance, update_vector); + } + } else { + RF_DEBUG( + "Could not transform measurement into " << target_frame << + ". Ignoring..."); + } + + RF_DEBUG("\n----- /UkfWrapper::prepareTwist (" << topic_name << ") ------\n"); + + return can_transform; +} + +void UkfWrapper::saveFilterState(robot_localization::Ukf & filter) +{ + FilterStatePtr state = FilterStatePtr(new FilterState()); + state->state_ = Eigen::VectorXd(filter.getState()); + state->estimate_error_covariance_ = + Eigen::MatrixXd(filter.getEstimateErrorCovariance()); + state->last_measurement_time_ = filter.getLastMeasurementTime(); + state->latest_control_ = Eigen::VectorXd(filter.getControl()); + state->latest_control_time_ = filter.getControlTime(); + filter_state_history_.push_back(state); + RF_DEBUG( + "Saved state with timestamp " << + std::setprecision(20) << + filter_utilities::toSec(state->last_measurement_time_) << + " to history. " << filter_state_history_.size() << + " measurements are in the queue.\n"); +} + +bool UkfWrapper::revertTo(const rclcpp::Time & time) +{ + RF_DEBUG("\n----- UkfWrapper::revertTo -----\n"); + RF_DEBUG( + "\nRequested time was " << std::setprecision(20) << + filter_utilities::toSec(time) << "\n") + + // size_t history_size = filter_state_history_.size(); + + // Walk back through the queue until we reach a filter state whose time stamp + // is less than or equal to the requested time. Since every saved state after + // that time will be overwritten/corrected, we can pop from the queue. If the + // history is insufficiently short, we just take the oldest state we have. + FilterStatePtr last_history_state; + while (!filter_state_history_.empty() && + filter_state_history_.back()->last_measurement_time_ > time) + { + last_history_state = filter_state_history_.back(); + filter_state_history_.pop_back(); + } + + // If the state history is not empty at this point, it means that our history + // was large enough, and we should revert to the state at the back of the + // history deque. + bool ret_val = false; + if (!filter_state_history_.empty()) { + ret_val = true; + last_history_state = filter_state_history_.back(); + } else { + RF_DEBUG( + "Insufficient history to revert to time " << + filter_utilities::toSec(time) << "\n"); + + if (last_history_state) { + RF_DEBUG( + "Will revert to oldest state at " << + filter_utilities::toSec(last_history_state->latest_control_time_) << + ".\n"); + + // ROS_WARN_STREAM_DELAYED_THROTTLE(history_length_, "Could not revert " + // "to state with time " << std::setprecision(20) << time << + // ". Instead reverted to state with time " << + // lastHistoryState->lastMeasurementTime_ << ". History size was " << + // history_size); + } + } + + // If we have a valid reversion state, revert + if (last_history_state) { + // Reset filter to the latest state from the queue. + const FilterStatePtr & state = last_history_state; + filter_.setState(state->state_); + filter_.setEstimateErrorCovariance(state->estimate_error_covariance_); + filter_.setLastMeasurementTime(state->last_measurement_time_); + + RF_DEBUG( + "Reverted to state with time " << + filter_utilities::toSec(state->last_measurement_time_) << "\n"); + + // Repeat for measurements, but push every measurement onto the measurement + // queue as we go + int restored_measurements = 0; + while (!measurement_history_.empty() && + measurement_history_.back()->time_ > time) + { + // Don't need to restore measurements that predate our earliest state time + if (state->last_measurement_time_ <= measurement_history_.back()->time_) { + measurement_queue_.push(measurement_history_.back()); + restored_measurements++; + } + + measurement_history_.pop_back(); + } + + RF_DEBUG( + "Restored " << restored_measurements << " to measurement queue." + "\n"); + } + + RF_DEBUG("\n----- /UkfWrapper::revertTo\n"); + + return ret_val; +} + +bool UkfWrapper::validateFilterOutput(nav_msgs::msg::Odometry * message) +{ + return !std::isnan(message->pose.pose.position.x) && + !std::isinf(message->pose.pose.position.x) && + !std::isnan(message->pose.pose.position.y) && + !std::isinf(message->pose.pose.position.y) && + !std::isnan(message->pose.pose.position.z) && + !std::isinf(message->pose.pose.position.z) && + !std::isnan(message->pose.pose.orientation.x) && + !std::isinf(message->pose.pose.orientation.x) && + !std::isnan(message->pose.pose.orientation.y) && + !std::isinf(message->pose.pose.orientation.y) && + !std::isnan(message->pose.pose.orientation.z) && + !std::isinf(message->pose.pose.orientation.z) && + !std::isnan(message->pose.pose.orientation.w) && + !std::isinf(message->pose.pose.orientation.w) && + !std::isnan(message->twist.twist.linear.x) && + !std::isinf(message->twist.twist.linear.x) && + !std::isnan(message->twist.twist.linear.y) && + !std::isinf(message->twist.twist.linear.y) && + !std::isnan(message->twist.twist.linear.z) && + !std::isinf(message->twist.twist.linear.z) && + !std::isnan(message->twist.twist.angular.x) && + !std::isinf(message->twist.twist.angular.x) && + !std::isnan(message->twist.twist.angular.y) && + !std::isinf(message->twist.twist.angular.y) && + !std::isnan(message->twist.twist.angular.z) && + !std::isinf(message->twist.twist.angular.z); +} + +void UkfWrapper::clearExpiredHistory(const rclcpp::Time cutoff_time) +{ + RF_DEBUG( + "\n----- UkfWrapper::clearExpiredHistory -----" << + "\nCutoff time is " << filter_utilities::toSec(cutoff_time) << + "\n"); + + int popped_measurements = 0; + int popped_states = 0; + + while (!measurement_history_.empty() && + measurement_history_.front()->time_ < cutoff_time) + { + measurement_history_.pop_front(); + popped_measurements++; + } + + while (!filter_state_history_.empty() && + filter_state_history_.front()->last_measurement_time_ < cutoff_time) + { + filter_state_history_.pop_front(); + popped_states++; + } + + RF_DEBUG( + "\nPopped " << popped_measurements << " measurements and " << + popped_states << + " states from their respective queues." << + "\n---- /UkfWrapper::clearExpiredHistory ----\n"); +} + +void UkfWrapper::clearMeasurementQueue() +{ + // Clear the measurement queue. + // This prevents us from immediately undoing our reset. + while (!measurement_queue_.empty() && rclcpp::ok()) { + measurement_queue_.pop(); + } +} +} // namespace robot_localization diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..8d470925 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -0,0 +1,12 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_localizer REQUIRED) + +ament_add_gtest(fusion_localizer_tests fusion_localizer_tests.cpp) + +target_link_libraries(fusion_localizer_tests + fusion_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp new file mode 100644 index 00000000..d43ae9a9 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -0,0 +1,158 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendFusionLocalizer : public easynav::FusionLocalizer +{ +public: + using easynav::FusionLocalizer::init_pose_sub_; + using easynav::FusionLocalizer::update_rt; +}; + +class FusionLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.5; + const double y0 = -0.25; + const double yaw0 = 0.7; + + const double x1 = -0.8; + const double y1 = 2.1; + const double yaw1 = -1.2; + + // Provide at least one parameter override so FusionLocalizer considers a filter configured. + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("test.global_filter.frequency", 30.0), + }); + + auto node = std::make_shared(options); + node->declare_parameter>("localizer_types", std::vector{}); + + auto localizer = std::make_shared(); + + ASSERT_NO_THROW(localizer->initialize(node, "test")); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("fusion_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < connect_deadline && + initialpose_pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + easynav::NavState nav_state; + + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; + init_pose_msg.header.stamp = pub_node->now(); + init_pose_msg.header.frame_id = "map"; + init_pose_msg.pose.pose.position.x = x1; + init_pose_msg.pose.pose.position.y = y1; + init_pose_msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + init_pose_msg.pose.pose.orientation = tf2::toMsg(q); + init_pose_msg.pose.covariance.fill(0.0); + initialpose_pub->publish(init_pose_msg); + + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update_rt(nav_state); + + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-6 && + std::abs(odom.pose.pose.position.y - y1) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-3; + + if (ok) { + break; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + ASSERT_TRUE(nav_state.has("robot_pose")); + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-3); + } +} diff --git a/localizers/easynav_gps_localizer/CHANGELOG.rst b/localizers/easynav_gps_localizer/CHANGELOG.rst index d08e6290..4f7d44e9 100644 --- a/localizers/easynav_gps_localizer/CHANGELOG.rst +++ b/localizers/easynav_gps_localizer/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package easynav_gps_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index 9d6e1cf7..0e5068c0 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/CMakeLists.txt @@ -5,16 +5,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_common REQUIRED) find_package(pluginlib REQUIRED) find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) # Geographiclib installs FindGeographicLib.cmake to this non-standard location set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") @@ -27,8 +30,15 @@ set(dependencies rclcpp rclcpp_lifecycle easynav_core + easynav_common pluginlib geographic_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros ) add_library(gps_localizer SHARED @@ -40,10 +50,17 @@ target_include_directories(gps_localizer PUBLIC ) target_link_libraries(gps_localizer easynav_core::easynav_core + easynav_common::easynav_common pluginlib::pluginlib ${geographic_msgs_TARGETS} ${GeographicLib_LIBRARIES} ${GeographicLib_LIBRARIES} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros ) install(DIRECTORY include/ @@ -63,6 +80,9 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) endif() ament_export_include_directories(include) diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index a989cfa3..97640b1f 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -1,21 +1,25 @@ # easynav_gps_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_gps_localizer/GpsLocalizer` - **Type:** `easynav::GpsLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,33 +27,43 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- - **Description:** GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. ## Parameters -This plugin does not declare configurable ROS parameters. + +All parameters are declared under the plugin namespace, i.e., `//easynav_gps_localizer/GpsLocalizer/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | +| `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | +| `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `robot/gps/fix` | `sensor_msgs/msg/NavSatFix` | Raw GPS fix. | SensorDataQoS (reliable) | | Subscription | `imu/data` | `sensor_msgs/msg/Imu` | IMU orientation for yaw fusion. | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the localizer pose from an external initial pose estimate. | depth=10 | | Publisher | `robot/odom_gps` | `nav_msgs/msg/Odometry` | Odometry fused from GPS + IMU (UTM-projected). | SensorDataQoS | - ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | GPS-based odometry estimate. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map (or world) -> odom_gps` | Static or slowly varying transform aligning UTM to local odometry frame. | | Optional | `base_link -> imu` | Used implicitly via IMU orientation; not looked up explicitly in this plugin. | - ## License -GPL-3.0-only + +Apache-2.0 diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index 4085b448..2628d6a9 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the GpsLocalizer class, a default plugin implementation for localization. @@ -23,16 +19,14 @@ #ifndef EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ #define EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ -#include #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "easynav_core/LocalizerMethodBase.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_ros/static_transform_broadcaster.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "tf2/LinearMath/Quaternion.hpp" -#include "tf2/LinearMath/Matrix3x3.hpp" #include +#include namespace easynav { @@ -63,9 +57,9 @@ class GpsLocalizer : public easynav::LocalizerMethodBase * * This override may be used to set up internal resources. By default, it simply succeeds. * - * @return std::expected Returns success or an error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the localization estimate based on the current navigation state. @@ -91,6 +85,16 @@ class GpsLocalizer : public easynav::LocalizerMethodBase */ virtual void update(NavState & nav_state) override; +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Initial pose received but not yet applied (e.g. waiting for a valid GPS fix). + std::optional pending_init_pose_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + private: /** * @brief Internal representation of the robot's current odometry. diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 6180c2e0..00de8bf8 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,11 +2,11 @@ easynav_gps_localizer - 0.0.2 + 0.4.0 Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake @@ -17,6 +17,12 @@ pluginlib geographiclib geographic_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros ament_lint_auto ament_lint_common diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index a50a7161..2d33a4f6 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -1,41 +1,41 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the GpsLocalizer class. -#include #include "easynav_gps_localizer/GpsLocalizer.hpp" #include "easynav_common/RTTFBuffer.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + namespace easynav { -std::expected GpsLocalizer::on_initialize() +void GpsLocalizer::on_initialize() { auto node = get_node(); + const auto & plugin_name = get_plugin_name(); // Initialize the odometry message odom_.header.stamp = get_node()->now(); - odom_.header.frame_id = get_tf_prefix() + "map"; - odom_.child_frame_id = get_tf_prefix() + "base_link"; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_.header.frame_id = tf_info.map_frame; + odom_.child_frame_id = tf_info.robot_frame; // Create subscriber to GPS data gps_subscriber_ = node->create_subscription( @@ -50,6 +50,38 @@ std::expected GpsLocalizer::on_initialize() "imu/data", rclcpp::SensorDataQoS().reliable(), std::bind(&GpsLocalizer::imu_callback, this, std::placeholders::_1)); + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&GpsLocalizer::init_pose_callback, this, std::placeholders::_1)); + + // Optional initial pose from parameters (kept consistent with AMCL parameter names) + node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + + double init_x = 0.0; + double init_y = 0.0; + double init_yaw = 0.0; + node->get_parameter(plugin_name + ".initial_pose.x", init_x); + node->get_parameter(plugin_name + ".initial_pose.y", init_y); + node->get_parameter(plugin_name + ".initial_pose.yaw", init_yaw); + + if (std::abs(init_x) > 1e-12 || std::abs(init_y) > 1e-12 || std::abs(init_yaw) > 1e-12) { + geometry_msgs::msg::PoseWithCovarianceStamped init_pose; + init_pose.header.stamp = node->now(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + init_pose.header.frame_id = tf_info.map_frame; + init_pose.pose.pose.position.x = init_x; + init_pose.pose.pose.position.y = init_y; + init_pose.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, init_yaw); + init_pose.pose.pose.orientation = tf2::toMsg(q); + init_pose.pose.covariance.fill(0.0); + pending_init_pose_ = init_pose; + } + // Create publisher odom_pub_ = node->create_publisher( "robot/odom_gps", rclcpp::SensorDataQoS().reliable()); @@ -57,8 +89,8 @@ std::expected GpsLocalizer::on_initialize() // Create static transform geometry_msgs::msg::TransformStamped transform; transform.header.stamp = node->now(); - transform.header.frame_id = get_tf_prefix() + "map"; - transform.child_frame_id = get_tf_prefix() + "odom"; + transform.header.frame_id = tf_info.map_frame; + transform.child_frame_id = tf_info.odom_frame; transform.transform.translation.x = 0.0; transform.transform.translation.y = 0.0; transform.transform.translation.z = 0.0; @@ -72,8 +104,6 @@ std::expected GpsLocalizer::on_initialize() time_1_ = get_node()->now().seconds(); alpha_ = 0.99; - - return {}; } void GpsLocalizer::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) @@ -87,6 +117,12 @@ void GpsLocalizer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) imu_msg_ = std::move(*msg); } +void GpsLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + pending_init_pose_ = *msg; +} + void GpsLocalizer::update_rt([[maybe_unused]] NavState & nav_state) { @@ -104,6 +140,14 @@ void GpsLocalizer::update(NavState & nav_state) GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, utm_x, utm_y); std::string utm_zone = std::to_string(zone) + (northp ? "N" : "S"); + // If an initial pose was provided, align the UTM origin so that the current + // GPS fix maps to that pose in the map frame. + if (pending_init_pose_.has_value() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { + origin_utm_.x = utm_x - pending_init_pose_->pose.pose.position.x; + origin_utm_.y = utm_y - pending_init_pose_->pose.pose.position.y; + pending_init_pose_.reset(); + } + if (origin_utm_ == geometry_msgs::msg::Point() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { @@ -114,8 +158,9 @@ void GpsLocalizer::update(NavState & nav_state) // Get XY cartesian coordinates respect to the origin odom_.header.stamp = gps_msg_.header.stamp; - odom_.header.frame_id = get_tf_prefix() + "map"; - odom_.child_frame_id = get_tf_prefix() + "base_link"; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_.header.frame_id = tf_info.map_frame; + odom_.child_frame_id = tf_info.robot_frame; odom_.pose.pose.position.x = utm_x - origin_utm_.x; odom_.pose.pose.position.y = utm_y - origin_utm_.y; diff --git a/localizers/easynav_gps_localizer/tests/CMakeLists.txt b/localizers/easynav_gps_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..0e8b0048 --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/CMakeLists.txt @@ -0,0 +1,10 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +ament_add_gtest(gps_localizer_tests gps_localizer_tests.cpp) + +target_link_libraries(gps_localizer_tests + gps_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp new file mode 100644 index 00000000..109266e2 --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp @@ -0,0 +1,187 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "easynav_gps_localizer/GpsLocalizer.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +namespace +{ + +class FriendGpsLocalizer : public easynav::GpsLocalizer +{ +public: + using easynav::GpsLocalizer::init_pose_sub_; + using easynav::GpsLocalizer::update; +}; + +class GpsLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(GpsLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.1; + const double y0 = -0.7; + + const double x1 = -2.3; + const double y1 = 0.4; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", 0.0), + }); + + auto node = std::make_shared("test_gps_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("gps_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + auto gps_pub = pub_node->create_publisher( + "robot/gps/fix", rclcpp::SensorDataQoS().reliable()); + auto imu_pub = pub_node->create_publisher( + "imu/data", rclcpp::SensorDataQoS().reliable()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < connect_deadline && + (initialpose_pub->get_subscription_count() == 0 || + gps_pub->get_subscription_count() == 0 || + imu_pub->get_subscription_count() == 0)) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + sensor_msgs::msg::Imu imu_msg; + imu_msg.header.stamp = pub_node->now(); + imu_msg.header.frame_id = ""; + imu_msg.angular_velocity.z = 0.0; + imu_msg.orientation.w = 1.0; + imu_pub->publish(imu_msg); + + sensor_msgs::msg::NavSatFix gps_msg; + gps_msg.header.stamp = pub_node->now(); + gps_msg.header.frame_id = ""; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + gps_msg.latitude = 42.0; + gps_msg.longitude = -1.0; + gps_msg.altitude = 0.0; + gps_pub->publish(gps_msg); + + easynav::NavState nav_state; + + // Deliver GPS/IMU callbacks and run update() until the initial pose is applied. + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update(nav_state); + + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x0) < 1e-9 && + std::abs(odom.pose.pose.position.y - y0) < 1e-9; + if (ok) { + break; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + ASSERT_TRUE(nav_state.has("robot_pose")); + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + } + + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; + init_pose_msg.header.stamp = pub_node->now(); + init_pose_msg.header.frame_id = "map"; + init_pose_msg.pose.pose.position.x = x1; + init_pose_msg.pose.pose.position.y = y1; + init_pose_msg.pose.pose.position.z = 0.0; + init_pose_msg.pose.pose.orientation.w = 1.0; + init_pose_msg.pose.covariance.fill(0.0); + initialpose_pub->publish(init_pose_msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update(nav_state); + + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9; + + if (ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + } +} diff --git a/localizers/easynav_navmap_localizer/CHANGELOG.rst b/localizers/easynav_navmap_localizer/CHANGELOG.rst index 37dca43e..2b25c821 100644 --- a/localizers/easynav_navmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_navmap_localizer/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package easynav_navmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Check if std is non-positive before creating std::normal_distribution +* Update README.md for all localizers +* Reset pose from RViz2 for every localizer +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index b55471df..37056907 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(navmap_core REQUIRED) @@ -17,6 +13,7 @@ find_package(navmap_ros_interfaces REQUIRED) find_package(easynav_localizer REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_bonxai_maps_manager REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -24,6 +21,8 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(Eigen3 REQUIRED NO_MODULE) add_library(${PROJECT_NAME} SHARED @@ -32,6 +31,7 @@ add_library(${PROJECT_NAME} SHARED target_include_directories(${PROJECT_NAME} PUBLIC $ $ + ${PCL_INCLUDE_DIRS} ) target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common @@ -40,12 +40,15 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_bonxai_maps_manager::easynav_bonxai_maps_manager ${navmap_ros_interfaces_TARGETS} easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib + Eigen3::Eigen ${tf2_geometry_msgs_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + ${PCL_LIBRARIES} ) install( @@ -68,7 +71,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") @@ -86,11 +89,14 @@ ament_export_dependencies( easynav_localizer easynav_core easynav_bonxai_maps_manager + easynav_sensors tf2 tf2_ros pluginlib geometry_msgs std_srvs nav_msgs + PCL + eigen ) ament_package() diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index 93ce6657..827a6c1c 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -1,21 +1,25 @@ # easynav_navmap_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Easy Navigation: nAVmAP Localizer package. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_navmap_localizer/AMCLLocalizer` - **Type:** `easynav::navmap::AMCLLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,6 +27,7 @@ Easy Navigation: nAVmAP Localizer package. - **Description:** A default "navmap" implementation for the AMCL localizer. ## Parameters + All parameters are declared under the plugin name namespace, i.e., `//easynav_navmap_localizer/AMCLLocalizer/...`. | Name | Type | Default | Description | @@ -50,16 +55,20 @@ All parameters are declared under the plugin name namespace, i.e., `// ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Subscription | `/odom` | `nav_msgs/msg/Odometry` | Used only when .compute_odom_from_tf = false | SensorDataQoS (reliable) | +| Subscription | `/odom` | `nav_msgs/msg/Odometry` | Used only when `.compute_odom_from_tf = false` | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize particles pose to the received pose, using covariance | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Publishes the estimated pose with covariance | depth=10 | ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `imu` | `IMUPerceptions` | **Read** | Used to get the latest IMU orientation when available. | @@ -70,11 +79,13 @@ This package does not create service servers or clients. | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose stored at the end of update/predict. | ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map -> odom` | Broadcasts the global transform that aligns the odometry frame with the map frame. | -| Requires | `odom -> base_footprint` | Required only when .compute_odom_from_tf = true (read from TF). | +| Requires | `odom -> base_footprint` | Required only when `.compute_odom_from_tf = true` (read from TF). | | Requires | `base_footprint -> ` | Required to transform perception point clouds into the robot base frame during correction. | ## License -GPL-3.0-only + +Apache-2.0 diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index fbfd32a9..55a666d3 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the AMCLLocalizer method. @@ -24,13 +20,9 @@ #define EASYNAV_NAVMAP_LOCALIZER__AMCLLOCALIZER_HPP_ #include -#include -#include -#include -#include -#include #include #include +#include #include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" @@ -79,9 +71,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -171,6 +163,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -178,6 +173,13 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + /** * @brief Update odom from TFs instead of a odom topic * @@ -188,7 +190,7 @@ class AMCLLocalizer : public LocalizerMethodBase std::vector particles_; /// Random number generator used for sampling noise. - std::default_random_engine rng_; + std::mt19937 rng_; /// Current estimated odometry-based pose. tf2::Transform pose_; @@ -234,11 +236,13 @@ class AMCLLocalizer : public LocalizerMethodBase /// Timestamp of the last reseed event. rclcpp::Time last_reseed_; + /// Timestamp of the last input message (odometry or initial pose). + rclcpp::Time last_input_time_; + /** * @brief Internal static map. */ std::shared_ptr bonxai_map_; - ::navmap::NavMap navmap_; ::navmap::NavCelId last_cid_ {0}; // PerceptionModel percepcion_model_; diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp index 4089e0c5..ffd56be4 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp @@ -1,22 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ #define EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 34c0d79e..3c0e38e8 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,10 +2,10 @@ easynav_navmap_localizer - 0.0.2 + 0.4.0 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake @@ -16,12 +16,15 @@ easynav_bonxai_maps_manager easynav_localizer easynav_core + easynav_sensors pluginlib tf2 tf2_ros tf2_geometry_msgs geometry_msgs nav_msgs + pcl_ros + eigen rclcpp rclcpp_lifecycle diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 21ba7c46..7f99c641 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -1,26 +1,21 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). -#include #include #include #include @@ -28,7 +23,6 @@ #include #include #include -#include #include #include @@ -40,9 +34,8 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" #include "navmap_core/NavMap.hpp" @@ -172,13 +165,13 @@ static inline tf2::Vector3 to_tf(const Eigen::Vector3f & v) } // ===================== probabilistic inflate (brushfire) ===================== -using ProgressCallback = std::function; +using ProgressCallback = std::function; /// Expect the fixed comma value returned by logods() [[nodiscard]] static constexpr float prob(int32_t logods_fixed) { float logods = float(logods_fixed) * 1e-6; - return 1.0 - 1.0 / (1.0 + std::exp(logods)); + return 1.0 - 1.0 / (1.0 + std::exp(logods)); } std::shared_ptr @@ -241,7 +234,7 @@ inflate_map( const auto clamp_min = dst->options().clamp_min_log; const auto clamp_max = dst->options().clamp_max_log; - auto write_max = [&](const CoordT & c, int32_t v){ + auto write_max = [&](const CoordT & c, int32_t v) { if (v == std::numeric_limits::min()) {return;} auto * cell = acc.value(c, true); int32_t prop = std::max(cell->probability_log, v); @@ -352,14 +345,17 @@ struct CoordEq // ---------- AMCLLocalizer ---------- AMCLLocalizer::AMCLLocalizer() +: rng_(std::random_device{}()) { NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { std::ostringstream ret; - tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); + tf2::Quaternion q( + odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); double r, p, y; tf2::Matrix3x3(q).getRPY(r, p, y); - ret << "Odometry with pose: (x: " << odom.pose.pose.position.x + ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " << + odom.pose.pose.position.x << ", y: " << odom.pose.pose.position.y << ", yaw: " << y << ")"; return ret.str(); }); @@ -367,7 +363,7 @@ AMCLLocalizer::AMCLLocalizer() AMCLLocalizer::~AMCLLocalizer() = default; -std::expected AMCLLocalizer::on_initialize() +void AMCLLocalizer::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -404,8 +400,9 @@ std::expected AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); node->get_parameter(plugin_name + ".noise_translation", noise_translation_); node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); + node->get_parameter( + plugin_name + ".noise_translation_to_rotation", + noise_translation_to_rotation_); node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); node->get_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); @@ -420,6 +417,22 @@ std::expected AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".top_keep_fraction", top_keep_fraction_); node->get_parameter(plugin_name + ".downsampled_cloud_size", downsampled_cloud_size_); + + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + + if (!std::isfinite(inflation_stddev_) || inflation_stddev_ <= 0.0) {inflation_stddev_ = 1.5;} if (!std::isfinite(inflation_prob_min_) || inflation_prob_min_ <= 0.0) { inflation_prob_min_ = 0.01; @@ -433,10 +446,12 @@ std::expected AMCLLocalizer::on_initialize() reseed_time_ = 1.0 / reseed_freq; RCLCPP_INFO(node->get_logger(), "Initialized AMCL with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "init pose (%.3f, %.3f, %.3f) std_dev [%.3f, %.3f]", + RCLCPP_INFO( + node->get_logger(), "init pose (%.3f, %.3f, %.3f) std_dev [%.3f, %.3f]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - RCLCPP_INFO(node->get_logger(), "inflation_stddev=%.3f, inflation_prob_min=%.3f, " - "correct_max_points=%zu, weights_tau=%.3f, top_keep_fraction=%.3f", + RCLCPP_INFO( + node->get_logger(), "inflation_stddev=%.3f, inflation_prob_min=%.3f, " + "correct_max_points=%zu, weights_tau=%.3f, top_keep_fraction=%.3f", inflation_stddev_, inflation_prob_min_, correct_max_points_, weights_tau_, top_keep_fraction_); std::normal_distribution noise_x(x_init, std_dev_xy); @@ -454,7 +469,8 @@ std::expected AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; if (!compute_odom_from_tf_) { @@ -464,42 +480,197 @@ std::expected AMCLLocalizer::on_initialize() } particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", + 10); estimate_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); + + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); last_reseed_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - return {}; } void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) { if (compute_odom_from_tf_) {return;} tf2::fromMsg(msg->pose.pose, odom_); + last_input_time_ = msg->header.stamp; if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, yaw_stddev); + + const std::size_t N = particles_.size(); + for (std::size_t i = 0; i < N; ++i) { + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::update_odom_from_tf() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); + tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "TF failed: %s", ex.what()); return; } tf2::fromMsg(tf_msg.transform, odom_); + last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; } std::optional get_latest_imu_quat(const NavState & nav_state) { if (!nav_state.has("imu")) {return std::nullopt;} - const auto imus = nav_state.get("imu"); - if (imus.empty() || !imus.back()) {return std::nullopt;} - const auto & imu_msg = imus.back()->data; + const auto & imu = nav_state.get("imu"); + const auto & imu_msg = imu.data; tf2::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w); if (q.length2() < 1e-12) {return std::nullopt;} @@ -530,7 +701,9 @@ void AMCLLocalizer::predict(NavState & nav_state) tf2::Transform delta = last_odom_.inverseTimes(odom_); const bool have_navmap = nav_state.has("map.navmap"); - if (have_navmap) {navmap_ = nav_state.get<::navmap::NavMap>("map.navmap");} + + if (!have_navmap) {return;} + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); const auto imu_q_opt = get_latest_imu_quat(nav_state); @@ -541,17 +714,16 @@ void AMCLLocalizer::predict(NavState & nav_state) double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw); double rot_len = std::abs(yaw); - std::random_device rd; std::mt19937 gen(rd()); + std::normal_distribution n_dx(0.0, std::abs(dx) * noise_translation_); + std::normal_distribution n_dy(0.0, std::abs(dy) * noise_translation_); + std::normal_distribution n_dz(0.0, std::abs(dz) * noise_translation_); + std::normal_distribution n_yaw(0.0, + rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + + double noisy_y = yaw + n_yaw(rng_); for (auto & p : particles_) { - std::normal_distribution n_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution n_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution n_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution n_yaw(0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); - - tf2::Vector3 noisy_t(dx + n_dx(gen), dy + n_dy(gen), dz + n_dz(gen)); - double noisy_y = yaw + n_yaw(gen); + tf2::Vector3 noisy_t(dx + n_dx(rng_), dy + n_dy(rng_), dz + n_dz(rng_)); tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_y); p.pose = p.pose * tf2::Transform(noisy_q, noisy_t); @@ -566,9 +738,10 @@ void AMCLLocalizer::predict(NavState & nav_state) std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits::max(); Eigen::Vector3f bary, hit_eig; - const bool ok = navmap_.locate_navcel( - Eigen::Vector3f(static_cast(Pw.x()), static_cast(Pw.y()), - static_cast(Pw.z() + 0.5f)), + const bool ok = navmap.locate_navcel( + Eigen::Vector3f( + static_cast(Pw.x()), static_cast(Pw.y()), + static_cast(Pw.z())), sidx, cid, bary, &hit_eig, opts); if (ok) { @@ -577,7 +750,7 @@ void AMCLLocalizer::predict(NavState & nav_state) if (imu_q_opt.has_value()) { p.pose.setRotation(*imu_q_opt); } else { - const auto & cel = navmap_.navcels[cid]; + const auto & cel = navmap.navcels[cid]; tf2::Vector3 n_world = to_tf(cel.normal); double nlen = n_world.length(); if (nlen > 1e-9) { @@ -618,12 +791,14 @@ static inline std::string get_frame_id_from(const PointPerception & pp) } -static inline tf2::Transform lookup_bf_to_sensor(const std::string & sensor_frame) +static inline tf2::Transform lookup_bf_to_sensor( + const std::string & robot_frame, + const std::string & sensor_frame) { if (sensor_frame.empty()) {return tf2::Transform::getIdentity();} geometry_msgs::msg::TransformStamped tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "base_footprint", sensor_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); + robot_frame, sensor_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); tf2::Transform T; tf2::fromMsg(tf_msg.transform, T); return T; } @@ -687,12 +862,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - auto t0 = get_node()->now(); - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto perceptions = nav_state.get("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); @@ -701,12 +875,13 @@ void AMCLLocalizer::correct(NavState & nav_state) // Build inflated map once and cache in NavState if (!nav_state.has("map.bonxai.inflated")) { - const auto original_map = nav_state.get_ptr("map.bonxai"); + const auto & original_map = nav_state.get_ptr("map.bonxai"); auto logger = get_node()->get_logger(); auto clock = get_node()->get_clock(); auto progress_cb = [logger, clock](float f) { - RCLCPP_INFO_THROTTLE(logger, *clock, 2000, "Inflating... %.1f%%", - static_cast(f) * 100.0); + RCLCPP_INFO_THROTTLE( + logger, *clock, 2000, "Inflating... %.1f%%", + static_cast(f) * 100.0); }; std::shared_ptr inflated_map = inflate_map(original_map, inflation_stddev_, inflation_prob_min_, progress_cb); @@ -717,14 +892,12 @@ void AMCLLocalizer::correct(NavState & nav_state) nav_state.set("map.bonxai.inflated", inflated_map); } - const auto original_map = nav_state.get_ptr("map.bonxai"); - const auto inflated_map = nav_state.get_ptr("map.bonxai.inflated"); + const auto & original_map = nav_state.get_ptr("map.bonxai"); + const auto & inflated_map = nav_state.get_ptr("map.bonxai.inflated"); // Compute tail-skip coherent with inflation kernel const int TAIL_SKIP = compute_tail_skip(*original_map, inflation_stddev_, inflation_prob_min_); - auto t1 = get_node()->now(); - // Prepare per-sensor bundles (downsample/cap per sensor) std::vector bundles; bundles.reserve(perceptions.size()); for (const auto & h : perceptions) { @@ -743,21 +916,23 @@ void AMCLLocalizer::correct(NavState & nav_state) T_bf_sensor_cache.reserve(bundles.size()); for (const auto & b : bundles) { if (!T_bf_sensor_cache.count(b.frame_id)) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); try { - T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(b.frame_id); + T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor( + tf_info.robot_frame, + b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), - ex.what()); + RCLCPP_WARN( + get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), + ex.what()); T_bf_sensor_cache[b.frame_id] = tf2::Transform::getIdentity(); } } } - auto t2 = get_node()->now(); - // Score all particles against all sensors bool debug = false; for (auto & particle : particles_) { @@ -776,8 +951,6 @@ void AMCLLocalizer::correct(NavState & nav_state) particle.possible_hits += possible; } - auto t3 = get_node()->now(); - // Weights update with power tau for (auto & particle : particles_) { if (particle.possible_hits > 0) { @@ -798,18 +971,6 @@ void AMCLLocalizer::correct(NavState & nav_state) p.weight /= total_w; } } - - auto t4 = get_node()->now(); - - // Light profiling - std::cerr << "Prepare maps: " << std::fixed << std::setprecision(6) << (t1 - t0).seconds() << - " s\n"; - std::cerr << "Prep sensors: " << std::fixed << std::setprecision(6) << (t2 - t1).seconds() << - " s\n"; - std::cerr << "Scoring: " << std::fixed << std::setprecision(6) << (t3 - t2).seconds() << - " s\n"; - std::cerr << "Weights: " << std::fixed << std::setprecision(6) << (t4 - t3).seconds() << - " s\n"; } // ------------------- reseed ------------------- @@ -827,16 +988,10 @@ AMCLLocalizer::reseed() }; const std::size_t N_top = top_count(N); - std::sort(particles_.begin(), particles_.end(), + std::sort( + particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) {return a.weight > b.weight;}); - // std::cerr << - // "=================================================================================\n"; - // for (std::size_t i = 0; i < particles_.size(); i++) { - // std::cerr << "[" << i << "] " << particles_[i].hits << "/" << particles_[i].possible_hits - // << " " << static_cast(particles_[i].last_cid) << std::endl; - // } - tf2::Vector3 mean = computeMean(particles_, 0, N_top); tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); @@ -976,9 +1131,10 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) return; } geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; + tf_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + tf_msg.header.frame_id = tf_info.map_frame; + tf_msg.child_frame_id = tf_info.odom_frame; tf_msg.transform = tf2::toMsg(map2bf); RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); tf_broadcaster_->sendTransform(tf_msg); @@ -986,9 +1142,11 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) void AMCLLocalizer::publishParticles() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; + array_msg.header.stamp = last_input_time_; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { geometry_msgs::msg::Pose pose_msg; @@ -1001,34 +1159,91 @@ void AMCLLocalizer::publishParticles() particles_pub_->publish(array_msg); } -tf2::Transform AMCLLocalizer::getEstimatedPose() const +tf2::Transform +AMCLLocalizer::getEstimatedPose() const { if (particles_.empty()) {return tf2::Transform::getIdentity();} - const std::size_t N = particles_.size(), N_top = N / 2; - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - tf2::Vector3 mean = computeMean(sorted, 0, N_top); + + const std::size_t N = particles_.size(); + std::size_t N_top = N / 2; + if (N_top == 0) { + N_top = 1; + } + + std::vector idx(N); + std::iota(idx.begin(), idx.end(), 0); + + std::nth_element( + idx.begin(), + idx.begin() + N_top, + idx.end(), + [this](std::size_t a, std::size_t b) { + return particles_[a].weight > particles_[b].weight; + }); + + std::vector top_particles; + top_particles.reserve(N_top); + for (std::size_t k = 0; k < N_top; ++k) { + top_particles.push_back(particles_[idx[k]]); + } + + tf2::Vector3 mean = computeMean(top_particles, 0, top_particles.size()); if (!std::isfinite(mean.x()) || !std::isfinite(mean.y()) || !std::isfinite(mean.z())) { mean = tf2::Vector3(0, 0, 0); } - double r, p, y; tf2::Matrix3x3(sorted[0].pose.getRotation()).getRPY(r, p, y); - tf2::Quaternion q; q.setRPY(0.0, 0.0, y); - tf2::Transform est; est.setOrigin(mean); est.setRotation(q); return est; + + const Particle & best = top_particles[0]; + double r, p, y; + tf2::Matrix3x3(best.pose.getRotation()).getRPY(r, p, y); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, y); + + tf2::Transform est; + est.setOrigin(mean); + est.setRotation(q); + return est; } -void AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) + +void +AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) { if (particles_.empty()) {return;} - const std::size_t N = particles_.size(), N_top = N / 2; + + const std::size_t N = particles_.size(); + std::size_t N_top = N / 2; + if (N_top == 0) { + N_top = 1; + } + + std::vector idx(N); + std::iota(idx.begin(), idx.end(), 0); + + std::nth_element( + idx.begin(), + idx.begin() + N_top, + idx.end(), + [this](std::size_t a, std::size_t b) { + return particles_[a].weight > particles_[b].weight; + }); + + std::vector top_particles; + top_particles.reserve(N_top); + for (std::size_t k = 0; k < N_top; ++k) { + top_particles.push_back(particles_[idx[k]]); + } tf2::Vector3 mean = est_pose.getOrigin(); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - double yaw_var = computeYawVariance(particles_, 0, N_top); + + tf2::Matrix3x3 cov = computeCovariance(top_particles, 0, top_particles.size(), mean); + double yaw_var = computeYawVariance(top_particles, 0, top_particles.size()); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.stamp = last_input_time_; + msg.header.frame_id = tf_info.map_frame; + msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); msg.pose.pose.position.z = mean.z(); @@ -1039,32 +1254,57 @@ void AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) msg.pose.covariance[6 * r + c] = cov[r][c]; } } + msg.pose.covariance[6 * 5 + 5] = yaw_var; + estimate_pub_->publish(msg); } -nav_msgs::msg::Odometry AMCLLocalizer::get_pose() +nav_msgs::msg::Odometry +AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; + odom_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_msg.header.frame_id = tf_info.map_frame; + odom_msg.child_frame_id = tf_info.robot_frame; pose_ = getEstimatedPose(); tf2::Transform est_pose = pose_; + odom_msg.pose.pose.position.x = est_pose.getOrigin().x(); odom_msg.pose.pose.position.y = est_pose.getOrigin().y(); odom_msg.pose.pose.position.z = est_pose.getOrigin().z(); odom_msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); if (!particles_.empty()) { - const std::size_t N = particles_.size(), N_top = N / 2; - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - tf2::Vector3 mean = computeMean(sorted, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(sorted, 0, N_top, mean); - double yaw_var = computeYawVariance(sorted, 0, N_top); + const std::size_t N = particles_.size(); + std::size_t N_top = N / 2; + if (N_top == 0) { + N_top = 1; + } + + std::vector idx(N); + std::iota(idx.begin(), idx.end(), 0); + + std::nth_element( + idx.begin(), + idx.begin() + N_top, + idx.end(), + [this](std::size_t a, std::size_t b) { + return particles_[a].weight > particles_[b].weight; + }); + + std::vector top_particles; + top_particles.reserve(N_top); + for (std::size_t k = 0; k < N_top; ++k) { + top_particles.push_back(particles_[idx[k]]); + } + + tf2::Vector3 mean = computeMean(top_particles, 0, top_particles.size()); + tf2::Matrix3x3 cov = computeCovariance(top_particles, 0, top_particles.size(), mean); + double yaw_var = computeYawVariance(top_particles, 0, top_particles.size()); + for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { odom_msg.pose.covariance[6 * r + c] = cov[r][c]; @@ -1079,6 +1319,7 @@ nav_msgs::msg::Odometry AMCLLocalizer::get_pose() odom_msg.twist.twist.angular.x = 0.0; odom_msg.twist.twist.angular.y = 0.0; odom_msg.twist.twist.angular.z = 0.0; + return odom_msg; } diff --git a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt index b874c059..400b4523 100644 --- a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt @@ -2,8 +2,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_srvs REQUIRED) -ament_add_gtest(costmap_localizer_tests costmap_localizer_tests.cpp) -target_link_libraries(costmap_localizer_tests +ament_add_gtest(navmap_localizer_tests navmap_localizer_tests.cpp) +target_link_libraries(navmap_localizer_tests ${PROJECT_NAME} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp deleted file mode 100644 index a40c68e9..00000000 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. -// -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - -#include - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_navmap_common/costmap_2d.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "std_srvs/srv/trigger.hpp" - -#include -#include - -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test -{ -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) -{ - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) -} - -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) -{ - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto pub = node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); -} - -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { -public: - void force_path(const std::string & path) {map_path_ = path;} -}; - -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) -{ - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); - - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); - - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); - - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); - - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); - - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); -} diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp new file mode 100644 index 00000000..7b454a4e --- /dev/null +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -0,0 +1,183 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "easynav_navmap_localizer/AMCLLocalizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +double yaw_from_tf(const tf2::Transform & tf) +{ + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendAMCLLocalizer : public easynav::navmap::AMCLLocalizer +{ +public: + using easynav::navmap::AMCLLocalizer::init_pose_sub_; +}; + +class AMCLLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = -3.0; + const double y0 = 0.25; + const double yaw0 = 0.2; + + const double x1 = 0.8; + const double y1 = 3.3; + const double yaw1 = 2.0; + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared( + "test_navmap_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); + } + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } +} diff --git a/localizers/easynav_simple_localizer/CHANGELOG.rst b/localizers/easynav_simple_localizer/CHANGELOG.rst index 22209583..d1595d98 100644 --- a/localizers/easynav_simple_localizer/CHANGELOG.rst +++ b/localizers/easynav_simple_localizer/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package easynav_simple_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Avoid creating objects every loop cycle +* Fix std::normal_distribution when stdev is zero +* Navstate key among filters are always map, not an arbitrary key +* Check if std is non-positive before creating std::normal_distribution +* Navstate key among filters are always map, not an arbitrary key +* Update README.md for all localizers +* Reset pose from RViz2 for every localizer +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 5423c281..85189806 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -5,15 +5,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_simple_common REQUIRED) find_package(easynav_localizer REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -21,6 +18,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) add_library(${PROJECT_NAME} SHARED @@ -34,9 +32,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_simple_common::easynav_simple_common easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib + Eigen3::Eigen ${tf2_geometry_msgs_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -62,7 +62,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") @@ -77,11 +77,13 @@ ament_export_dependencies( easynav_simple_common easynav_localizer easynav_core + easynav_sensors tf2 tf2_ros pluginlib geometry_msgs std_srvs nav_msgs + eigen ) ament_package() diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 04505e89..428f59cf 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -1,21 +1,25 @@ # easynav_simple_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + AMCL-style localizer using a simple 2D grid-like map for scoring and odometry for prediction. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_simple_localizer/AMCLLocalizer` - **Type:** `easynav::AMCLLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,6 +27,7 @@ AMCL-style localizer using a simple 2D grid-like map for scoring and odometry fo - **Description:** AMCL-style localizer using a simple 2D grid-like map for scoring and odometry for prediction. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_simple_localizer/AMCLLocalizer/...`. | Name | Type | Default | Description | @@ -40,33 +45,35 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.min_noise_xy` | `double` | `0.05` | Minimum XY noise (m). | | `.min_noise_yaw` | `double` | `0.05` | Minimum yaw noise (rad). | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false (not present here). | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize particles pose to the received pose, using covariance. | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set. | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Estimated pose with covariance. | depth=10 | - ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | -| `map.static` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | +| `map.base` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map -> odom` | Aligns the odometry frame with the map frame. | - ## License -GPL-3.0-only + +Apache-2.0 diff --git a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 22f60ab9..caf6fdd7 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the AMCLLocalizer method. @@ -24,11 +20,6 @@ #define EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_ #include -#include -#include -#include -#include -#include #include #include @@ -72,9 +63,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -164,6 +155,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -171,6 +165,13 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + /// List of particles representing the belief distribution. std::vector particles_; @@ -209,6 +210,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Timestamp of the last reseed event. rclcpp::Time last_reseed_; + + /// Timestamp of the last input message (odometry or initial pose). + rclcpp::Time last_input_time_; }; } // namespace easynav diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 93492165..8936b16d 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,10 +2,10 @@ easynav_simple_localizer - 0.0.2 + 0.4.0 Easy Navigation: Simple Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake @@ -13,12 +13,14 @@ easynav_simple_common easynav_localizer easynav_core + easynav_sensors pluginlib tf2 tf2_ros tf2_geometry_msgs geometry_msgs nav_msgs + eigen rclcpp rclcpp_lifecycle diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 32891a95..3f9674e2 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -1,34 +1,28 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the AMCLLocalizer class. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_simple_localizer/AMCLLocalizer.hpp" @@ -177,7 +171,8 @@ AMCLLocalizer::AMCLLocalizer() double roll, pitch, yaw; tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; + ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " << + x << ", y: " << y << ", yaw: " << yaw << ")"; return ret.str(); }); } @@ -186,7 +181,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -216,22 +211,39 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); node->get_parameter(plugin_name + ".noise_translation", noise_translation_); node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", + node->get_parameter( + plugin_name + ".noise_translation_to_rotation", noise_translation_to_rotation_); node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + + double reseed_freq; node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); reseed_time_ = 1.0 / reseed_freq; RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", + RCLCPP_INFO( + node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -248,7 +260,8 @@ AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; @@ -257,15 +270,19 @@ AMCLLocalizer::on_initialize() std::bind(&AMCLLocalizer::odom_callback, this, _1), options); particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", + 10); estimate_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); + + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); last_reseed_ = get_node()->now(); + last_input_time_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - - return {}; } void printTransform(const tf2::Transform & tf) @@ -312,6 +329,7 @@ void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) { tf2::fromMsg(msg->pose.pose, odom_); + last_input_time_ = msg->header.stamp; if (!initialized_odom_) { last_odom_ = odom_; @@ -319,6 +337,154 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) } } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); + + const std::size_t N = particles_.size(); + for (std::size_t i = 0; i < N; ++i) { + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) { @@ -339,15 +505,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) std::random_device rd; std::mt19937 gen(rd()); - for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - - std::normal_distribution noise_yaw( - 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(gen), dy + noise_dy(gen), @@ -373,26 +541,26 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto perceptions = nav_state.get("points"); - - if (!nav_state.has("map.static")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); + if (!nav_state.has("map.base")) { + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map"); return; } - const auto & map_static = nav_state.get("map.static"); + const auto & map_static = nav_state.get("map.base"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) - .fuse(get_tf_prefix() + "base_footprint") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .fuse(tf_info.robot_footprint_frame, last_input_time_) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) - ->downsample(map_static.resolution()) + .downsample(map_static.resolution()) .as_points(); if (filtered.empty()) { @@ -454,7 +622,8 @@ AMCLLocalizer::reseed() const std::size_t N = particles_.size(); const std::size_t N_top = N / 2; - std::sort(particles_.begin(), particles_.end(), + std::sort( + particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -473,7 +642,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { @@ -491,8 +660,9 @@ AMCLLocalizer::reseed() double dx = l00 * z0; double dy = l10 * z0 + l11 * z1; - std::normal_distribution xy_noise(0.0, std::max(sqrt(dx * dx + dy * dy), - min_noise_xy_)); + std::normal_distribution xy_noise(0.0, std::max( + sqrt(dx * dx + dy * dy), + min_noise_xy_)); tf2::Vector3 new_origin(origin.x() + xy_noise(rng_), origin.y() + xy_noise(rng_), 0.0); @@ -526,9 +696,10 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; + tf_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + tf_msg.header.frame_id = tf_info.map_frame; + tf_msg.child_frame_id = tf_info.odom_frame; tf_msg.transform = tf2::toMsg(map2bf); RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); @@ -538,9 +709,11 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf) void AMCLLocalizer::publishParticles() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; + array_msg.header.stamp = last_input_time_; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -569,7 +742,8 @@ AMCLLocalizer::getEstimatedPose() const const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -599,9 +773,11 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); double yaw_variance = computeYawVariance(particles_, 0, N_top); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.stamp = last_input_time_; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -623,9 +799,10 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; + odom_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_msg.header.frame_id = tf_info.map_frame; + odom_msg.child_frame_id = tf_info.robot_frame; tf2::Transform est_pose = getEstimatedPose(); @@ -639,7 +816,8 @@ AMCLLocalizer::get_pose() const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index abf70ba0..d122a9ec 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -1,184 +1,182 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include -#include "easynav_simple_common/SimpleMap.hpp" +#include +#include +#include +#include + #include "easynav_simple_localizer/AMCLLocalizer.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -/// \brief Dynamic map update tests -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->initialize(30, 30, 0.1, -1.5, -1.5, 0.0); - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - - perception->data.points.resize(6); - perception->data.points[0].x = 1.0; - perception->data.points[0].y = 1.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = -1.0; - perception->data.points[1].y = -1.0; - perception->data.points[1].z = 0.2; - perception->data.points[2].x = -10.0; - perception->data.points[2].y = -1.0; - perception->data.points[2].z = 0.2; - perception->data.points[3].x = 10.0; - perception->data.points[3].y = -1.0; - perception->data.points[3].z = 0.2; - perception->data.points[4].x = 1.0; - perception->data.points[4].y = -10.0; - perception->data.points[4].z = 0.2; - perception->data.points[5].x = 1.0; - perception->data.points[5].y = 10.0; - perception->data.points[5].z = 0.2; - - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_dynamyc_map()); - ASSERT_TRUE(map_ptr != nullptr); - - auto cell1 = map_ptr->metric_to_cell(1.0, 1.0); - EXPECT_TRUE(map_ptr->at(cell1.first, cell1.second)); - - auto cell2 = map_ptr->metric_to_cell(-1.0, -1.0); - EXPECT_TRUE(map_ptr->at(cell2.first, cell2.second)); + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -/// \brief Map loading via subscription test -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - rclcpp::Publisher::SharedPtr pub = - node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; // Mark cell (5,5) as occupied - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - - EXPECT_EQ(static_map->at(5, 5), 1); - EXPECT_EQ(static_map->at(1, 1), 0); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - auto static_map = std::make_shared(); - static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); - static_map->at(1, 1) = true; - static_map->at(2, 2) = true; - manager->set_static_map(static_map); +} // namespace + +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 0.3; + const double y0 = 1.7; + const double yaw0 = -0.8; - const std::string test_map_file = "/tmp/savemap_test_map.txt"; - const std::string service_name = "/test_savemap_node/test_savemap/savemap"; + const double x1 = 2.2; + const double y1 = -0.4; + const double yaw1 = 1.1; - std::static_pointer_cast(manager)->force_path(test_map_file); + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), + }); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); + auto node = std::make_shared( + "test_simple_localizer_node", options); + auto localizer = std::make_shared(); - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + localizer->initialize(node, "test"); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + ASSERT_NE(localizer->init_pose_sub_, nullptr); - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); + } - std::ifstream infile(test_map_file); - ASSERT_TRUE(infile.is_open()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); + } - std::string first_line; - std::getline(infile, first_line); - EXPECT_NE(first_line.find("4 4"), std::string::npos); - EXPECT_NE(first_line.find("0.5"), std::string::npos); + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - infile.close(); - std::remove(test_map_file.c_str()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst index 77e5495a..e7537fc2 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package easynav_bonxai_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Update calls to deprecated get_package_share_directory +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index 994a44ea..90476e36 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) @@ -19,6 +15,8 @@ find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io filters) find_package(pcl_conversions REQUIRED) find_package(std_srvs REQUIRED) @@ -59,6 +57,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -109,6 +108,7 @@ pluginlib_export_plugin_description_file(easynav_core easynav_bonxai_maps_manage ament_export_dependencies( easynav_common easynav_core + rclcpp tf2 tf2_ros pluginlib diff --git a/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 4df2fba2..a01fb940 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -1,21 +1,25 @@ # easynav_bonxai_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) probabilistic 3D occupancy map and exposes it via ROS topics, NavState, and a save-map service. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_bonxai_maps_manager/BonxaiMapsManager` - **Type:** `easynav_bonxai::BonxaiMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -25,6 +29,7 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## Parameters ### Plugin Parameters (namespace: `//easynav_bonxai_maps_manager/BonxaiMapsManager/...`) + | Name | Type | Default | Description | |---|---|---:|---| | `.package` | `string` | `""` | Package name to resolve relative map paths via ament index. | @@ -33,34 +38,34 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) | `.resolution` | `double` | `0.3` | Voxel resolution for the Bonxai map (meters). | | `.frame_id` | `string` | `"map"` | Frame ID stamped on published map messages. | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update the Bonxai map. | QoS depth=100 | | Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid (YAML + PGM) to import into Bonxai. | QoS depth=1, transient_local, reliable | | Publisher | `//map` | `sensor_msgs/msg/PointCloud2` | Published Bonxai map as a point cloud. | QoS depth=1, transient_local, reliable | - ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Save current Bonxai map to disk. | - ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `map.bonxai` | `Bonxai::ProbabilisticMap` | **Write** | Stores the current Bonxai map instance. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `—` | No TF broadcast; outputs use the configured frame_id. | - ## License -GPL-3.0-only + +Apache-2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp index cf9fee79..82fd94fb 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp @@ -38,7 +38,8 @@ struct EmptyVoxel {}; * DIM will be 8 and SIZE 512 (8³) */ template -class Grid { +class Grid +{ private: uint8_t dim_ = 0; // total number of elements in the cube @@ -98,14 +99,14 @@ class Grid { [[nodiscard]] DataT & cell(size_t index) { static_assert( - !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); + !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); return data_[index]; } [[nodiscard]] const DataT & cell(size_t index) const { static_assert( - !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); + !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); return data_[index]; } }; @@ -121,7 +122,8 @@ enum ClearOption }; template -class VoxelGrid { +class VoxelGrid +{ private: uint32_t INNER_BITS; uint32_t LEAF_BITS; @@ -228,7 +230,8 @@ class VoxelGrid { // for special cases, such as serialization or testing std::shared_ptr allocateLeafGrid(); - class ConstAccessor { + class ConstAccessor + { public: ConstAccessor(const VoxelGrid & grid) : grid_(grid) {} @@ -273,7 +276,8 @@ class VoxelGrid { * * Create an instance of this object with the method VoxelGrid::createAccessor() */ - class Accessor : public ConstAccessor { + class Accessor : public ConstAccessor + { public: Accessor(VoxelGrid & grid) : ConstAccessor(grid), @@ -481,9 +485,9 @@ template inline bool VoxelGrid::Accessor::setValue(const CoordT & coord, const DataT & value) { static_assert( - !std::is_same_v, - "You can not access a value when using type EmptyVoxel. Use " - "setCellOn / setCellOff"); + !std::is_same_v, + "You can not access a value when using type EmptyVoxel. Use " + "setCellOn / setCellOff"); const CoordT inner_key = mutable_grid_.getInnerKey(coord); if (inner_key != prev_inner_coord_ || prev_leaf_ptr_ == nullptr) { @@ -502,9 +506,9 @@ template inline DataT * VoxelGrid::Accessor::value(const CoordT & coord, bool create_if_missing) { static_assert( - !std::is_same_v, - "You can not access a value when using type EmptyVoxel. Use " - "isCellOn / setCellOn / setCellOff"); + !std::is_same_v, + "You can not access a value when using type EmptyVoxel. Use " + "isCellOn / setCellOn / setCellOff"); const CoordT inner_key = mutable_grid_.getInnerKey(coord); @@ -530,8 +534,8 @@ template inline const DataT * VoxelGrid::ConstAccessor::value(const CoordT & coord) const { static_assert( - !std::is_same_v, - "You can not access a value when using type EmptyVoxel. Use isCellOn"); + !std::is_same_v, + "You can not access a value when using type EmptyVoxel. Use isCellOn"); const CoordT inner_key = grid_.getInnerKey(coord); diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp index 4c7ab7e7..5da57f3b 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp @@ -27,7 +27,8 @@ namespace Bonxai * Each chunk allocates memory for 512 Grids */ template -class GridBlockAllocator { +class GridBlockAllocator +{ public: // use log2dim of the block to be allocated GridBlockAllocator(size_t log2dim); @@ -38,7 +39,7 @@ class GridBlockAllocator { GridBlockAllocator & operator=(const GridBlockAllocator & other) = delete; GridBlockAllocator & operator=(GridBlockAllocator && other) = default; - using Deleter = std::function; + using Deleter = std::function; std::pair allocateBlock(); @@ -133,11 +134,12 @@ inline void GridBlockAllocator::releaseUnusedMemory() { std::unique_lock lock(*mutex_); int to_be_erased_count = 0; - auto remove_if = std::remove_if(chunks_.begin(), chunks_.end(), [&](const auto & chunk) -> bool { - bool notUsed = chunk->mask.isOn(); - to_be_erased_count += (notUsed) ? 1 : 0; - return notUsed; - }); + auto remove_if = std::remove_if( + chunks_.begin(), chunks_.end(), [&](const auto & chunk) -> bool { + bool notUsed = chunk->mask.isOn(); + to_be_erased_count += (notUsed) ? 1 : 0; + return notUsed; + }); chunks_.erase(remove_if, chunks_.end()); capacity_ -= to_be_erased_count * blocks_per_chunk; } diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp index ca1f2b28..264d0831 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp @@ -143,17 +143,19 @@ template inline PointOut ConvertPoint(const PointIn & v) { // clang-format off - static_assert(std::is_same_v|| - type_has_method_x::value || - type_has_member_x::value || - type_is_vector::value, - "Can't convert from the specified type"); - - static_assert(std::is_same_v|| - type_has_method_x::value || - type_has_member_x::value || - type_is_vector::value, - "Can't convert to the specified type"); + static_assert( + std::is_same_v|| + type_has_method_x::value || + type_has_member_x::value || + type_is_vector::value, + "Can't convert from the specified type"); + + static_assert( + std::is_same_v|| + type_has_method_x::value || + type_has_member_x::value || + type_is_vector::value, + "Can't convert to the specified type"); // clang-format on if constexpr (std::is_same_v) { diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp index dac2dc51..09c5957d 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp @@ -17,7 +17,8 @@ namespace Bonxai { -class Mask { +class Mask +{ uint64_t * words_ = nullptr; // small object optimization, that will be used when // SIZE <= 512 bits, i.e LOG2DIM <= 3 @@ -61,7 +62,8 @@ class Mask { uint32_t countOn() const; - class Iterator { + class Iterator + { public: Iterator(const Mask * parent) : pos_(parent->SIZE), @@ -268,7 +270,7 @@ inline uint32_t Mask::findNextOn(uint32_t start) const while (!b && ++n < WORD_COUNT) { b = words_[n]; } // find next non-zero word - return !b ? SIZE : (n << 6) + FindLowestOn(b); // catch last word=0 + return !b ? SIZE : (n << 6) + FindLowestOn(b); // catch last word=0 } inline Mask::Mask(size_t log2dim) diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp index 741b54ed..c056f3ca 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp @@ -14,10 +14,11 @@ void RayIterator(const CoordT & key_origin, const CoordT & key_end, const Functo inline void ComputeRay(const CoordT & key_origin, const CoordT & key_end, std::vector & ray) { ray.clear(); - RayIterator(key_origin, key_end, [&ray](const CoordT & coord) { + RayIterator( + key_origin, key_end, [&ray](const CoordT & coord) { ray.push_back(coord); return true; - }); + }); } /** @@ -26,7 +27,8 @@ inline void ComputeRay(const CoordT & key_origin, const CoordT & key_end, std::v * * Insert a point cloud to update the current probability */ -class ProbabilisticMap { +class ProbabilisticMap +{ public: using Vector3D = Eigen::Vector3d; @@ -42,7 +44,7 @@ class ProbabilisticMap { [[nodiscard]] static constexpr float prob(int32_t logods_fixed) { float logods = float(logods_fixed) * 1e-6; - return 1.0 - 1.0 / (1.0 + std::exp(logods)); + return 1.0 - 1.0 / (1.0 + std::exp(logods)); } struct CellT diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp index 0eecce6c..d7440b29 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp @@ -89,8 +89,8 @@ inline void Serialize(std::ostream & out, const VoxelGrid & grid) std::string type_name = details::demangle(typeid(DataT).name()); sprintf( - header, "Bonxai::VoxelGrid<%s,%d,%d>(%lf)\n", type_name.c_str(), grid.innetBits(), - grid.leafBits(), grid.voxelSize()); + header, "Bonxai::VoxelGrid<%s,%d,%d>(%lf)\n", type_name.c_str(), grid.innetBits(), + grid.leafBits(), grid.voxelSize()); out.write(header, std::strlen(header)); diff --git a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp index 5d0d84c4..4130fccb 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the BonxaiMapsManager method. @@ -23,29 +19,14 @@ #ifndef EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ #define EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ -#include -#include -#include -#include -#include -#include - -#include "bonxai/bonxai.hpp" #include "bonxai/probabilistic_map.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" -#include "pluginlib/class_loader.hpp" - -#include "yaets/tracing.hpp" - namespace easynav_bonxai { @@ -76,9 +57,9 @@ class BonxaiMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -133,7 +114,6 @@ class BonxaiMapsManager : public easynav::MapsManagerBase double resolution_ {0.3}; double height_with_occ_ {1.0}; - std::string frame_id_ {"map"}; }; } // namespace easynav_bonxai diff --git a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp deleted file mode 100644 index 8e582124..00000000 --- a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#ifndef EASYNAV_BONXAI_MAPS_MANAGER_UTILS_HPP_ -#define EASYNAV_BONXAI_MAPS_MANAGER_UTILS_HPP_ - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav_bonxai -{ - - -} // namespace easynav_bonxai -#endif // EASYNAV_BONXAI_MAPS_MANAGER_UTILS_HPP_ diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 46c75db4..d1abc1da 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -2,15 +2,16 @@ easynav_bonxai_maps_manager - 0.0.2 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake easynav_common easynav_core + rclcpp tf2 tf2_ros tf2_geometry_msgs diff --git a/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp b/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp index 036d9a5b..7f168809 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp @@ -86,7 +86,7 @@ bool ProbabilisticMap::isFree(const CoordT & coord) const float ProbabilisticMap::queryProbability(const Bonxai::CoordT & coord) const { - if (const CellT * cell = _accessor.value(coord, /*create=*/false)) { + if (const CellT * cell = _accessor.value(coord, /*create=*/ false)) { float p = ProbabilisticMap::prob(cell->probability_log); return std::clamp(p, 0.0f, 1.0f); } diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp index 3d34d36a..4f32a408 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp @@ -1,30 +1,22 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include #include "easynav_bonxai_maps_manager/BonxaiMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/YTSession.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "bonxai/bonxai.hpp" @@ -34,7 +26,6 @@ #include "pcl_conversions/pcl_conversions.h" #include "pcl/point_types.h" #include "pcl/point_cloud.h" -#include "pcl/filters/voxel_grid.h" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -57,7 +48,7 @@ BonxaiMapsManager::BonxaiMapsManager() BonxaiMapsManager::~BonxaiMapsManager() {} -std::expected +void BonxaiMapsManager::on_initialize() { auto node = get_node(); @@ -68,16 +59,14 @@ BonxaiMapsManager::on_initialize() node->declare_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->declare_parameter(plugin_name + ".resolution", resolution_); - node->declare_parameter(plugin_name + ".frame_id", frame_id_); node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); node->get_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->get_parameter(plugin_name + ".resolution", resolution_); - node->get_parameter(plugin_name + ".frame_id", frame_id_); bonxai_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); map_path_ = "/tmp/bonxai_map.pcd"; @@ -87,7 +76,7 @@ BonxaiMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + bonxai_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } std::vector bonxai_result; @@ -99,11 +88,12 @@ BonxaiMapsManager::on_initialize() if (!std::isfinite(p.x()) || !std::isfinite(p.y()) || !std::isfinite(p.z())) {continue;} bonxai_map_->addHitPoint(p); - pcl_out.push_back({ + pcl_out.push_back( + { static_cast(p.x()), static_cast(p.y()), static_cast(p.z()) - }); + }); } bonxai_msg_.data.clear(); @@ -120,13 +110,13 @@ BonxaiMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } nav_msgs::msg::OccupancyGrid occ_msg; if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::make_unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } update_from_occ(occ_msg); @@ -134,7 +124,8 @@ BonxaiMapsManager::on_initialize() } incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { update_from_pc2(*msg); @@ -142,7 +133,8 @@ BonxaiMapsManager::on_initialize() }); incoming_occ_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { @@ -151,7 +143,7 @@ BonxaiMapsManager::on_initialize() }); savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/savemap", [this]( const std::shared_ptr request, std::shared_ptr response) @@ -165,8 +157,6 @@ BonxaiMapsManager::on_initialize() Bonxai::WritePointsFromPCD(map_path_, bonxai_result); // This can overwrite yaml occ maps // ToDo }); - - return {}; } void @@ -181,11 +171,13 @@ BonxaiMapsManager::update(::easynav::NavState & nav_state) void BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) { + const auto & tf_info = ::easynav::RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = ::easynav::RTTFBuffer::getInstance()->lookupTransform( - frame_id_, pc2.header.frame_id, pc2.header.stamp, rclcpp::Duration::from_seconds(0.05)); + tf_info.map_frame, pc2.header.frame_id, pc2.header.stamp, + rclcpp::Duration::from_seconds(0.05)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "OctomapMapsManager: TF failed: %s", ex.what()); return; @@ -213,11 +205,12 @@ BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) bonxai_result.clear(); bonxai_map_->getOccupiedVoxels(bonxai_result); for (const auto & voxel : bonxai_result) { - pcl_out.push_back({ + pcl_out.push_back( + { static_cast(voxel.x()), static_cast(voxel.y()), static_cast(voxel.z()) - }); + }); } bonxai_msg_.data.clear(); @@ -275,11 +268,12 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) bonxai_result.clear(); bonxai_map_->getOccupiedVoxels(bonxai_result); for (const auto & voxel : bonxai_result) { - pcl_out.push_back({ + pcl_out.push_back( + { static_cast(voxel.x()), static_cast(voxel.y()), static_cast(voxel.z()) - }); + }); } bonxai_msg_.data.clear(); @@ -289,7 +283,9 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) void BonxaiMapsManager::publish_map() { - bonxai_msg_.header.frame_id = frame_id_; + const auto & tf_info = ::easynav::RTTFBuffer::getInstance()->get_tf_info(); + + bonxai_msg_.header.frame_id = tf_info.map_frame; bonxai_msg_.header.stamp = this->get_node()->now(); bonxai_pub_->publish(bonxai_msg_); diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp deleted file mode 100644 index 59339434..00000000 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "easynav_bonxai_maps_manager/utils.hpp" - -namespace easynav_bonxai -{ - - -} // namespace easynav_bonxai diff --git a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst index 7e45f896..e913dfbf 100644 --- a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package easynav_costmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Update dynamic_map\_ with map key at navstate +* Avoid rewrite map with dynamic_map +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Update calls to deprecated get_package_share_directory +* Update dynamic_map\_ with map key at navstate +* Navstate key among filters are always map, not an arbitrary key +* Any change in the costmap set it as modified +* Publish changes in base map and be ready to save updated map +* Costamp base map sincronized with nav_state +* Change map static to base +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Jose Miguel, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt index 6ef5ac5b..ac3b93bc 100644 --- a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt @@ -5,19 +5,21 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(easynav_costmap_common REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(std_srvs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaets REQUIRED) @@ -39,10 +41,15 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle tf2_ros::tf2_ros + tf2::tf2 + Eigen3::Eigen pluginlib::pluginlib ament_index_cpp::ament_index_cpp easynav_costmap_common::easynav_costmap_common + easynav_sensors::easynav_sensors yaets::yaets ${nav_msgs_TARGETS} ${std_srvs_TARGETS} @@ -86,11 +93,16 @@ pluginlib_export_plugin_description_file(easynav_costmap_maps_manager easynav_co ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2_ros + tf2 + eigen pluginlib nav_msgs ament_index_cpp easynav_costmap_common + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index e3a54d13..7acf6015 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -1,11 +1,11 @@ # easynav_costmap_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description -Maps Manager that maintains 2D costmaps (static and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration. + +Maps Manager that maintains 2D costmaps (base and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration. At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends the binary occupancy grid into a graded cost representation with values in the range [0–255]: + - 0: Free space, no cost to traverse. - 1–252: Gradual cost values, representing increasing difficulty or proximity to obstacles. - 253: “Near obstacle” (inscribed obstacle) cost, traversal strongly discouraged. @@ -13,16 +13,21 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends - 255: Unknown space. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_costmap_maps_manager/CostmapMapsManager` - **Type:** `easynav::CostmapMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -34,6 +39,7 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends ## Parameters ### Plugin Parameters (namespace: `//easynav_costmap_maps_manager/CostmapMapsManager/...`) + | Name | Type | Default | Description | |---|---|---:|---| | `.package` | `string` | `""` | Package name used to resolve relative map paths via `ament_index`. | @@ -43,19 +49,60 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends --- -### Filter Parameters -Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any specific parameters. +### Filter Plugins -#### InflationFilter -| Name | Type | Default | Description | +Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any filter-specific parameters. + +#### **ObstacleFilter** + +- **Plugin Name:** `easynav_costmap_maps_manager/ObstacleFilter` +- **Type:** `easynav::ObstacleFilter` +- **Description:** + Detects occupied cells from input point clouds (`points` key in `NavState`) and marks them as `LETHAL_OBSTACLE` in the dynamic costmap. + The filter fuses incoming 3D points into the map frame, downsamples them to the costmap resolution, filters out ground-level points (z < 0.1 m), and sets corresponding cells to lethal cost. Additionally, it computes and stores bounding box (`ObstacleBounds`) of updated obstacles to enable efficient incremental inflation. + +**Parameters:** + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| _(None)_ | — | — | This filter does not declare additional ROS parameters beyond `plugin`. Downsampling resolution and frame fusion use the costmap's own resolution and `map` frame. | + +**NavState Keys:** + +| Key | Type | Access | Description | +|---|---|---|---| +| `points` | `sensor_msgs::msg::PointCloud2` | **Read** | Input point clouds to detect obstacles. | +| `map` | `Costmap2D` | **Write** | Marks cells as `LETHAL_OBSTACLE` (254). | +| `map.obstacle_bounds` | `ObstacleBounds` | **Write** | Bounding box of updated obstacles for incremental inflation. | + +#### **InflationFilter** + +- **Plugin Name:** `easynav_costmap_maps_manager/InflationFilter` +- **Type:** `easynav::InflationFilter` +- **Description:** + Expands obstacle information in the costmap by assigning graded costs around `LETHAL_OBSTACLE` cells based on distance. Uses a breadth-first wavefront propagation algorithm (distance bins) to efficiently inflate obstacles up to `inflation_radius`. + The filter reads both the base map and the dynamic filtered map, applies inflation to each, and merges results. If `ObstacleBounds` is available in `NavState`, inflation is restricted to the updated region for performance. + +**Parameters:** + +| Parameter | Type | Default | Description | |---|---|---:|---| -| `.inflation.inflation_radius` | `double` | `0.3` | Radius (m) used to inflate obstacles in the costmap. | -| `.inflation.cost_scaling_factor` | `double` | `3.0` | Exponential decay factor controlling cost reduction with distance. | +| `.inflation_radius` | `double` | `0.3` | Maximum inflation distance (m) from obstacles. Cells farther than this receive no inflation cost. | +| `.inscribed_radius` | `double` | `0.25` | Radius of the inscribed zone (m). Cells within this distance of an obstacle are marked with high constant cost (`INSCRIBED_INFLATED_OBSTACLE`, value 253) before exponential decay begins. | +| `.cost_scaling_factor` | `double` | `3.0` | Exponential decay rate controlling how quickly cost decreases with distance beyond the inscribed radius. Higher values produce steeper cost gradients. | + +**NavState Keys:** -#### ObstacleFilter -This filter does not declare any ROS parameters apart from `plugin`. +| Key | Type | Access | Description | +|---|---|---|---| +| `map.base` | `Costmap2D` | **Read** | Base costmap to inflate. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. | +| `map.obstacle_bounds` | `ObstacleBounds` | **Read** (optional) | Restricts inflation to updated region for performance. | + +**Cost Model:** +Uses exponential decay: `cost = exp(-cost_scaling_factor * (distance - inscribed_radius)) * 253` for distances beyond inscribed radius. -**Example Configuration** +### Example Configuration ```yaml maps_manager_node: @@ -79,13 +126,15 @@ maps_manager_node: ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy map used to update the dynamic map. | `depth=1, transient_local, reliable` | -| Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Publishes the static costmap. | `depth=1` | +| Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Publishes the base costmap. | `depth=1` | | Publisher | `//dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Publishes the dynamic (live) costmap. | `depth=100` | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current costmap(s) to disk. | @@ -93,15 +142,17 @@ maps_manager_node: --- ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| -| `map.static` | `Costmap2D` | **Write** | Static map loaded from YAML. | -| `map.dynamic` | `Costmap2D` | **Write** | Dynamic map after applying filters. | -| `map.dynamic.filtered` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map.base` | `Costmap2D` | **Write** | Base map loaded from YAML. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic map after applying filters; if an existing filtered `map` is available, the manager may use it as input. | +| `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | — | This manager does not broadcast TF; costmaps use their internal `frame_id`. | @@ -109,4 +160,5 @@ maps_manager_node: --- ## License -GPL-3.0-only + +Apache-2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index 3499a0e6..cfef0727 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the CostmapMapsManager method. @@ -24,26 +20,16 @@ #define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ #include -#include -#include -#include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" #include "pluginlib/class_loader.hpp" -#include "yaets/tracing.hpp" - namespace easynav { @@ -73,9 +59,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -92,7 +78,7 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * @param new_map Shared pointer to a new map object. Must be of type SimpleMap. */ - void set_static_map(const Costmap2D & new_map); + void set_base_map(const Costmap2D & new_map); protected: /** @@ -101,15 +87,20 @@ class CostmapMapsManager : public easynav::MapsManagerBase std::string map_path_; private: + /** + * @brief Internal base map. + */ + Costmap2D map_base_; + /** * @brief Internal static map. */ - Costmap2D static_map_; + std::shared_ptr dynamic_map_; /** - * @brief Publisher for the static occupancy grid. + * @brief Publisher for the base occupancy grid. */ - rclcpp::Publisher::SharedPtr static_occ_pub_; + rclcpp::Publisher::SharedPtr base_occ_pub_; /** * @brief Publisher for the dynamic occupancy grid. @@ -127,9 +118,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase rclcpp::Service::SharedPtr savemap_srv_; /** - * @brief Cached occupancy grid message for the static map. + * @brief Cached occupancy grid message for the base map. */ - nav_msgs::msg::OccupancyGrid static_grid_msg_; + nav_msgs::msg::OccupancyGrid base_grid_msg_; /** * @brief Cached occupancy grid message for the dynamic map. diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp index 645fde85..f4ba6b9a 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp @@ -1,30 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ -#include #include -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -32,31 +25,33 @@ namespace easynav { +struct ObstacleBounds +{ + double min_x, min_y, max_x, max_y; +}; + + class CostmapFilter { public: CostmapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix = ""); + const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(NavState & nav_state) = 0; -protected: - std::shared_ptr get_node() const; - const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; +protected: + std::shared_ptr get_node() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; }; } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp index 56275fbc..9a6f9b32 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp @@ -42,14 +42,9 @@ #ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ -#include +#include #include -#include #include -#include -#include - -#include "pluginlib/class_loader.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -84,7 +79,7 @@ class InflationFilter : public CostmapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); /** @@ -202,7 +197,13 @@ class InflationFilter : public CostmapFilter } /** - * @brief Enqueue new cells in cache distance update search + * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation + * @param grid The costmap + * @param index The index of the cell + * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) + * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) + * @param src_x The x index of the obstacle point inflation started at + * @param src_y The y index of the obstacle point inflation started at */ inline void enqueue( unsigned int index, unsigned int mx, unsigned int my, @@ -229,6 +230,22 @@ class InflationFilter : public CostmapFilter bool need_reinflation_; bool matchedSize_ {false}; + + Costmap2D base_inflated_; + bool has_base_inflated_ {false}; + + struct BaseGeomSignature + { + unsigned int size_x {0}; + unsigned int size_y {0}; + double resolution {0.0}; + double origin_x {0.0}; + double origin_y {0.0}; + int64_t stamp_ns {0}; + } base_sig_; + + bool needs_recompute_base_(const Costmap2D & base_map) const; + void recompute_base_inflation_(const Costmap2D & base_map); }; } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp index a73d5848..4db115f0 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp @@ -1,32 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ -#include #include -#include "pluginlib/class_loader.hpp" - -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" @@ -39,7 +30,7 @@ class ObstacleFilter : public CostmapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); }; diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 186af843..b07fe435 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -2,22 +2,25 @@ easynav_costmap_maps_manager - 0.0.2 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake - + rclcpp + rclcpp_lifecycle easynav_common easynav_core tf2_ros + tf2 + eigen pluginlib nav_msgs ament_index_cpp easynav_costmap_common + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index 229f4e43..0d6d77c3 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -1,34 +1,28 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include +#include #include #include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" #include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_costmap_common/cost_values.hpp" #include "easynav_costmap_maps_manager/map_io.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -54,7 +48,7 @@ CostmapMapsManager::CostmapMapsManager() CostmapMapsManager::~CostmapMapsManager() {} -std::expected +void CostmapMapsManager::on_initialize() { auto node = get_node(); @@ -77,40 +71,48 @@ CostmapMapsManager::on_initialize() node->get_parameter(plugin_name + "." + costmap_filter + ".plugin", plugin); try { - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loading CostmapFilter %s [%s]", costmap_filter.c_str(), plugin.c_str()); std::shared_ptr instance; instance = costmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + costmap_filter, - get_tf_prefix()); - - if (!result) { - RCLCPP_ERROR(node->get_logger(), - "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - return std::make_unexpected("Unable to initialize " + - plugin + " . Error: " + result.error()); + try { + instance->initialize(node, plugin_name + "." + costmap_filter); + } catch (std::runtime_error & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); + throw std::runtime_error( + "Unable to initialize " + + plugin + " . Error: " + ex.what()); } costmap_filters_.push_back(instance); - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loaded CostmapFilter %s [%s]", costmap_filter.c_str(), plugin.c_str()); } catch (pluginlib::PluginlibException & ex) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to load plugin easynav::CostmapFilter. Error: %s", ex.what()); - return std::make_unexpected("Unable to load plugin easynav::CostmapFilter " + - costmap_filter + " . Error: " + ex.what()); + throw std::runtime_error( + "Unable to load plugin easynav::CostmapFilter " + + costmap_filter + " . Error: " + ex.what()); } } - static_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + base_occ_pub_ = node->create_publisher( + node->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); dynamic_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/dynamic_map", 100); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); map_path_ = "/tmp/default.map.yaml"; if (!package_name.empty() && !map_path_file.empty()) { @@ -118,38 +120,38 @@ CostmapMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + map_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } - if (auto ret = loadMapFromYaml(map_path_, static_grid_msg_) != LOAD_MAP_SUCCESS) { + if (auto ret = loadMapFromYaml(map_path_, base_grid_msg_) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::make_unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } - static_map_ = Costmap2D(static_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = node->now(); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; - static_grid_msg_.header.stamp = node->now(); - static_occ_pub_->publish(static_grid_msg_); + map_base_ = Costmap2D(base_grid_msg_); + base_occ_pub_->publish(base_grid_msg_); } incoming_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), - [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { - static_grid_msg_ = *msg; + [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { + base_grid_msg_ = *msg; - static_map_ = Costmap2D(*msg); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = this->get_node()->now(); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; - static_grid_msg_.header.stamp = this->get_node()->now(); + map_base_ = Costmap2D(base_grid_msg_); - static_occ_pub_->publish(static_grid_msg_); + base_occ_pub_->publish(base_grid_msg_); }); savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", - [this]( + node->get_name() + std::string("/") + plugin_name + "/savemap", + [&]( const std::shared_ptr request, std::shared_ptr response) { @@ -162,7 +164,7 @@ CostmapMapsManager::on_initialize() params.free_thresh = 0.25; params.occupied_thresh = 0.65; - if (!saveMapToFile(static_grid_msg_, params)) { + if (!saveMapToFile(base_grid_msg_, params)) { response->success = false; response->message = "Failed to save map to: " + map_path_; } else { @@ -170,14 +172,20 @@ CostmapMapsManager::on_initialize() response->message = "Map successfully saved to: " + map_path_; } }); - - return {}; } void -CostmapMapsManager::set_static_map(const Costmap2D & new_map) +CostmapMapsManager::set_base_map(const Costmap2D & new_map) { - static_map_ = new_map; + map_base_ = new_map; + + if (base_occ_pub_) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_base_.toOccupancyGridMsg(base_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = map_base_.getLastModifiedStamp(); + base_occ_pub_->publish(base_grid_msg_); + } } @@ -186,23 +194,56 @@ CostmapMapsManager::update(NavState & nav_state) { EASYNAV_TRACE_EVENT; - if (!nav_state.has("map.static")) { - nav_state.set("map.static", static_map_); + // Sync internal base map with NavState, preferring the newest stamp. + // Note: Compare using nanoseconds to avoid rclcpp::Time clock-type mismatches. + if (nav_state.has("map.base")) { + const auto & external_base = nav_state.get("map.base"); + + const int64_t internal_ns = map_base_.getLastModifiedStamp().nanoseconds(); + const int64_t external_ns = external_base.getLastModifiedStamp().nanoseconds(); + + if (external_ns > internal_ns) { + map_base_ = external_base; + + if (base_occ_pub_) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_base_.toOccupancyGridMsg(base_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = map_base_.getLastModifiedStamp(); + base_occ_pub_->publish(base_grid_msg_); + } + } else if (internal_ns > external_ns) { + nav_state.set("map.base", map_base_); + } + } else { + nav_state.set("map.base", map_base_); + } + + if (!dynamic_map_) { + dynamic_map_ = std::make_shared(map_base_); + } else { + *dynamic_map_ = map_base_; } - Costmap2D dynamic_map = static_map_; - nav_state.set("map.dynamic.filtered", dynamic_map); + nav_state.set("map", dynamic_map_); + + if (!nav_state.has("map_time")) { + nav_state.set("map_time", get_node()->now()); + } for (const auto & filter : costmap_filters_) { filter->update(nav_state); } - const auto & final_dynamic_map = nav_state.get("map.dynamic.filtered"); - nav_state.set("map.dynamic", final_dynamic_map); + *dynamic_map_ = nav_state.get("map"); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + rclcpp::Time map_stamp = nav_state.get("map_time"); - final_dynamic_map.toOccupancyGridMsg(dynamic_grid_msg_); - dynamic_grid_msg_.header.frame_id = get_tf_prefix() + "map"; - dynamic_grid_msg_.header.stamp = get_node()->now(); + dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_); + dynamic_grid_msg_.header.frame_id = tf_info.map_frame; + dynamic_grid_msg_.header.stamp = map_stamp; dynamic_occ_pub_->publish(dynamic_grid_msg_); } diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp index cfb320ec..4c4343ff 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp @@ -1,29 +1,20 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include -#include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_common/types/NavState.hpp" - #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" namespace easynav @@ -34,18 +25,15 @@ CostmapFilter::CostmapFilter() } -std::expected +void CostmapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; - return on_initialize(); + on_initialize(); } std::shared_ptr @@ -60,10 +48,5 @@ CostmapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -CostmapFilter::get_tf_prefix() const -{ - return tf_prefix_; -} } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index 372049e0..bde4337a 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -39,13 +39,10 @@ *********************************************************************/ -#include #include #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -73,35 +70,44 @@ InflationFilter::InflationFilter() { } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); inflation_radius_ = 0.3; + inscribed_radius_ = 0.25; cost_scaling_factor_ = 3.0; node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); - RCLCPP_INFO(node->get_logger(), - "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", - inflation_radius_, cost_scaling_factor_); + RCLCPP_INFO( + node->get_logger(), + "InflationFilter with inflation_radius = %lf inscribed_radius = %lf cost_scaling_factor = %lf", + inflation_radius_, inscribed_radius_, cost_scaling_factor_); seen_.clear(); cached_distances_.clear(); cached_costs_.clear(); need_reinflation_ = false; - - return {}; } void InflationFilter::update(NavState & nav_state) { - Costmap2D dynamic_map = nav_state.get("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); + Costmap2D & dynamic_map = *dynamic_map_ptr; + + const auto & base_map = nav_state.get("map.base"); + + if (needs_recompute_base_(base_map)) { + recompute_base_inflation_(base_map); + } if (!matchedSize_) { cell_inflation_radius_ = cellDistance(dynamic_map, inflation_radius_); @@ -109,11 +115,48 @@ InflationFilter::update(NavState & nav_state) matchedSize_ = true; } - updateCosts(dynamic_map, 0, 0, dynamic_map.getSizeInCellsX(), dynamic_map.getSizeInCellsY()); + int min_i = 0; + int min_j = 0; + int size_x = dynamic_map.getSizeInCellsX(); + int size_y = dynamic_map.getSizeInCellsY(); + int max_i = size_x; + int max_j = size_y; - nav_state.set("map.dynamic.filtered", dynamic_map); -} + if (nav_state.has("map.obstacle_bounds")) { + const auto & bb = nav_state.get("map.obstacle_bounds"); + + unsigned int cmin_i, cmin_j, cmax_i, cmax_j; + if (dynamic_map.worldToMap(bb.min_x, bb.min_y, cmin_i, cmin_j) && + dynamic_map.worldToMap(bb.max_x, bb.max_y, cmax_i, cmax_j)) + { + min_i = static_cast(cmin_i); + min_j = static_cast(cmin_j); + max_i = static_cast(cmax_i) + 1; + max_j = static_cast(cmax_j) + 1; + + // Expand by the inflation radius in cells, so that all cells whose + // cost may be affected by the new obstacles are included. + const int r = static_cast(cell_inflation_radius_); + min_i = std::max(0, min_i - r); + min_j = std::max(0, min_j - r); + max_i = std::min(max_i + r, size_x); + max_j = std::min(max_j + r, size_y); + } + } + + updateCosts(dynamic_map, min_i, min_j, max_i, max_j); + + for (int i = 0; i < dynamic_map.getSizeInCellsX(); i++) { + for (int j = 0; j < dynamic_map.getSizeInCellsY(); j++) { + int index = static_cast(dynamic_map.getIndex(i, j)); + unsigned char cost = std::max( + dynamic_map.getCost(i, j), base_inflated_.getCost(i, j)); + dynamic_map.setCost(i, j, cost); + } + } + nav_state.set("map", dynamic_map_ptr); +} void InflationFilter::matchSize(easynav::Costmap2D & costmap) @@ -156,107 +199,125 @@ InflationFilter::updateBounds( } } - void InflationFilter::updateCosts( - easynav::Costmap2D & master_grid, int min_i, int min_j, - int max_i, - int max_j) + easynav::Costmap2D & master_grid, + int min_i, int min_j, + int max_i, int max_j) { if (cell_inflation_radius_ == 0) { return; } - // make sure the inflation list is empty at the beginning of the cycle (should always be true) - for (auto & dist : inflation_cells_) { - RCLCPP_FATAL_EXPRESSION( - parent_node_->get_logger(), - !dist.empty(), "The inflation list must be empty at the beginning of inflation"); + // Early exit: no valid region to inflate + if (min_i >= max_i || min_j >= max_j) { + return; } unsigned char * master_array = master_grid.getCharMap(); - unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); - - if (seen_.size() != size_x * size_y) { - RCLCPP_WARN( - parent_node_->get_logger(), "InflationFilter::updateCosts(): seen_ vector size is wrong"); - seen_ = std::vector(size_x * size_y, false); + const unsigned int size_x = master_grid.getSizeInCellsX(); + const unsigned int size_y = master_grid.getSizeInCellsY(); + const std::size_t cell_count = static_cast(size_x) * size_y; + // Reuse boolean seen_ vector for this call to avoid full O(N) clearing + if (seen_.size() != cell_count) { + seen_.assign(cell_count, false); + } else { + // Clear only inside the current bounded region to reduce cost + for (int j = min_j; j < max_j; ++j) { + unsigned int row = static_cast(j) * size_x; + for (int i = min_i; i < max_i; ++i) { + seen_[row + static_cast(i)] = false; + } + } } - std::fill(begin(seen_), end(seen_), false); + // Reuse all inflation distance bins instead of reallocating them + for (auto & dist_bin : inflation_cells_) { + dist_bin.clear(); + } - // We need to include in the inflation cells outside the bounding - // box min_i...max_j, by the amount cell_inflation_radius_. Cells - // up to that distance outside the box can still influence the costs - // stored in cells inside the box. + // Store the original bounding box (before expansion by the inflation radius) const int base_min_i = min_i; const int base_min_j = min_j; const int base_max_i = max_i; const int base_max_j = max_j; + + // Expand the region by the inflation radius (in cells). Cells up to that + // distance outside the original box can still influence costs inside it. min_i -= static_cast(cell_inflation_radius_); min_j -= static_cast(cell_inflation_radius_); max_i += static_cast(cell_inflation_radius_); max_j += static_cast(cell_inflation_radius_); + // Clamp expanded region to map bounds min_i = std::max(0, min_i); min_j = std::max(0, min_j); max_i = std::min(static_cast(size_x), max_i); max_j = std::min(static_cast(size_y), max_j); - // Inflation list; we append cells to visit in a list associated with - // its distance to the nearest obstacle - // We use a map to emulate the priority queue used before, - // with a notable performance boost + if (min_i >= max_i || min_j >= max_j) { + return; + } - // Start with lethal obstacles: by definition distance is 0.0 + // -------------------------------------------------------------------------- + // 1) Collect seeds (lethal obstacles and optionally unknown cells) inside + // the expanded window. These go into distance bin 0. + // -------------------------------------------------------------------------- auto & obs_bin = inflation_cells_[0]; obs_bin.reserve(200); - for (int j = min_j; j < max_j; j++) { - for (int i = min_i; i < max_i; i++) { - int index = static_cast(master_grid.getIndex(i, j)); - unsigned char cost = master_array[index]; - if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { + + for (int j = min_j; j < max_j; ++j) { + for (int i = min_i; i < max_i; ++i) { + const unsigned int index = master_grid.getIndex(i, j); + const unsigned char cost = master_array[index]; + + if (cost == LETHAL_OBSTACLE || + (inflate_around_unknown_ && cost == NO_INFORMATION)) + { obs_bin.emplace_back(i, j, i, j); } } } - // Process cells by increasing distance; new cells are appended to the - // corresponding distance bin, so they - // can overtake previously inserted but farther away cells + // No seeds in this region → nothing to inflate + if (obs_bin.empty()) { + return; + } + + // -------------------------------------------------------------------------- + // 2) BFS over the distance bins. New cells are appended to the bin + // corresponding to their distance from the nearest obstacle. + // -------------------------------------------------------------------------- for (auto & dist_bin : inflation_cells_) { dist_bin.reserve(200); - for (std::size_t i = 0; i < dist_bin.size(); ++i) { - // Do not use iterator or for-range based loops to - // iterate though dist_bin, since it's size might - // change when a new cell is enqueued, invalidating all iterators - const CellData & cell = dist_bin[i]; - unsigned int mx = cell.x_; - unsigned int my = cell.y_; - unsigned int sx = cell.src_x_; - unsigned int sy = cell.src_y_; - unsigned int index = master_grid.getIndex(mx, my); - - // ignore if already visited + + for (std::size_t k = 0; k < dist_bin.size(); ++k) { + const CellData & cell = dist_bin[k]; + const unsigned int mx = cell.x_; + const unsigned int my = cell.y_; + const unsigned int sx = cell.src_x_; + const unsigned int sy = cell.src_y_; + const unsigned int index = master_grid.getIndex(mx, my); + // Process each cell only once in this call if (seen_[index]) { continue; } - seen_[index] = true; - // assign the cost associated with the distance from an obstacle to the cell - unsigned char cost = costLookup(mx, my, sx, sy); - unsigned char old_cost = master_array[index]; - // In order to avoid artifacts appeared out of boundary areas - // when some layer is going after inflation_layer, - // we need to apply inflation_layer only to inside of given bounds + // Compute inflation cost based on distance to the nearest obstacle + const unsigned char cost = costLookup(mx, my, sx, sy); + const unsigned char old_cost = master_array[index]; + + // Apply inflation only inside the original (non-expanded) bounding box if (static_cast(mx) >= base_min_i && static_cast(my) >= base_min_j && static_cast(mx) < base_max_i && static_cast(my) < base_max_j) { if (old_cost == NO_INFORMATION && - (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + (inflate_unknown_ ? + (cost > FREE_SPACE) : + (cost >= INSCRIBED_INFLATED_OBSTACLE))) { master_array[index] = cost; } else { @@ -264,35 +325,25 @@ InflationFilter::updateCosts( } } - // attempt to put the neighbors of the current cell onto the inflation list - if (mx > 0) { - enqueue(index - 1, mx - 1, my, sx, sy); + // Enqueue 4-connected neighbors (clipped to full map bounds) + if (mx > 0u) { + enqueue(index - 1u, mx - 1u, my, sx, sy); } - if (my > 0) { - enqueue(index - size_x, mx, my - 1, sx, sy); + if (my > 0u) { + enqueue(index - size_x, mx, my - 1u, sx, sy); } - if (mx < size_x - 1) { - enqueue(index + 1, mx + 1, my, sx, sy); + if (mx + 1u < size_x) { + enqueue(index + 1u, mx + 1u, my, sx, sy); } - if (my < size_y - 1) { - enqueue(index + size_x, mx, my + 1, sx, sy); + if (my + 1u < size_y) { + enqueue(index + size_x, mx, my + 1u, sx, sy); } } - // This level of inflation_cells_ is not needed anymore. We can free the memory - // Note that dist_bin.clear() is not enough, because it won't free the memory - dist_bin = std::vector(); + + // No need to shrink dist_bin here; capacity is reused on the next call. } } -/** - * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation - * @param grid The costmap - * @param index The index of the cell - * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) - * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) - * @param src_x The x index of the obstacle point inflation started at - * @param src_y The y index of the obstacle point inflation started at - */ void InflationFilter::enqueue( unsigned int index, unsigned int mx, unsigned int my, @@ -314,6 +365,7 @@ InflationFilter::enqueue( // push the cell data onto the inflation list and mark const auto dist = distance_matrix_[mx - src_x + r][my - src_y + r]; inflation_cells_[dist].emplace_back(mx, my, src_x, src_y); + // Do not mark seen_ here; we mark upon processing to allow enqueuing neighbors } } @@ -391,6 +443,48 @@ InflationFilter::generateIntegerDistances() return level; } +bool +InflationFilter::needs_recompute_base_(const Costmap2D & base_map) const +{ + if (!has_base_inflated_) { + return true; + } + + const int64_t stamp_ns = base_map.getLastModifiedStamp().nanoseconds(); + + if (base_map.getSizeInCellsX() != base_sig_.size_x || + base_map.getSizeInCellsY() != base_sig_.size_y || + base_map.getResolution() != base_sig_.resolution || + base_map.getOriginX() != base_sig_.origin_x || + base_map.getOriginY() != base_sig_.origin_y || + stamp_ns != base_sig_.stamp_ns) + { + return true; + } + + return false; +} + +void +InflationFilter::recompute_base_inflation_(const Costmap2D & base_map) +{ + base_inflated_ = base_map; + + matchSize(base_inflated_); + const int w = base_inflated_.getSizeInCellsX(); + const int h = base_inflated_.getSizeInCellsY(); + updateCosts(base_inflated_, 0, 0, w, h); + + base_sig_.size_x = base_map.getSizeInCellsX(); + base_sig_.size_y = base_map.getSizeInCellsY(); + base_sig_.resolution = base_map.getResolution(); + base_sig_.origin_x = base_map.getOriginX(); + base_sig_.origin_y = base_map.getOriginY(); + base_sig_.stamp_ns = base_map.getLastModifiedStamp().nanoseconds(); + + has_base_inflated_ = true; +} + } // namespace easynav #include diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index f6623a47..37dff4bb 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -1,30 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -40,37 +33,59 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(NavState & nav_state) { - if (!nav_state.has("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + return; + } + + auto dynamic_map_ptr = nav_state.get_ptr("map"); + if (!dynamic_map_ptr) { + RCLCPP_WARN(get_node()->get_logger(), "There is no map in NavState"); return; } + Costmap2D & dynamic_map = *dynamic_map_ptr; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + rclcpp::Time stamp; - const auto & perceptions = nav_state.get("points"); + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(dynamic_map.getResolution()) + .fuse(tf_info.map_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); - Costmap2D dynamic_map = nav_state.get("map.dynamic.filtered"); + const auto & fused = view.as_points(); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(dynamic_map.getResolution()) - .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .as_points(); + nav_state.set("map_time", stamp); + + double min_x = std::numeric_limits::max(); + double min_y = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + double max_y = std::numeric_limits::lowest(); for (const auto & p : fused) { + min_x = std::min(min_x, static_cast(p.x)); + min_y = std::min(min_y, static_cast(p.y)); + max_x = std::max(max_x, static_cast(p.x)); + max_y = std::max(max_y, static_cast(p.y)); + unsigned int cx, cy; if (dynamic_map.worldToMap(p.x, p.y, cx, cy)) { dynamic_map.setCost(cx, cy, LETHAL_OBSTACLE); } } - nav_state.set("map.dynamic.filtered", dynamic_map); + if (!fused.empty()) { + ObstacleBounds bb{min_x, min_y, max_x, max_y}; + nav_state.set("map.obstacle_bounds", bb); + } } } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index 398e5f63..c6e9997e 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -1,26 +1,35 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include +#include +#include +#include + +#include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" #include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" -#include "easynav_costmap_maps_manager/map_io.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" -#include -#include +#include "nav_msgs/msg/occupancy_grid.hpp" /// \brief Fixture for CostmapMapsManager tests class CostmapMapsManagerTest : public ::testing::Test @@ -29,17 +38,6 @@ class CostmapMapsManagerTest : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - easynav::NavState::register_printer( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override @@ -48,6 +46,156 @@ class CostmapMapsManagerTest : public ::testing::Test } }; +static nav_msgs::msg::OccupancyGrid make_grid( + int width, + int height, + int occ_x, + int occ_y, + int64_t stamp_ns) +{ + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = "map"; + grid.header.stamp.sec = static_cast(stamp_ns / 1000000000LL); + grid.header.stamp.nanosec = static_cast(stamp_ns % 1000000000LL); + + grid.info.width = static_cast(width); + grid.info.height = static_cast(height); + grid.info.resolution = 0.2f; + grid.info.origin.position.x = -1.0; + grid.info.origin.position.y = -1.0; + grid.data.assign(static_cast(width * height), 0); + + if (occ_x >= 0 && occ_y >= 0 && occ_x < width && occ_y < height) { + grid.data[static_cast(occ_y * width + occ_x)] = 100; + } + + return grid; +} + +TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerNavStateMap) +{ + auto node = std::make_shared("test_node_sync_ns"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + const int64_t older_ns = 10LL * 1000000000LL; + const int64_t newer_ns = 20LL * 1000000000LL; + + easynav::Costmap2D internal_old(make_grid(10, 10, 1, 1, older_ns)); + easynav::Costmap2D external_new(make_grid(10, 10, 2, 2, newer_ns)); + + manager->set_base_map(internal_old); + + easynav::NavState navstate; + navstate.set("map.base", external_new); + + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); + + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); + EXPECT_EQ(dyn_ptr->getCost(2, 2), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getCost(1, 1), easynav::FREE_SPACE); +} + +TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerInternalMap) +{ + auto node = std::make_shared("test_node_sync_internal"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + const int64_t older_ns = 10LL * 1000000000LL; + const int64_t newer_ns = 20LL * 1000000000LL; + + easynav::Costmap2D internal_new(make_grid(10, 10, 3, 3, newer_ns)); + easynav::Costmap2D external_old(make_grid(10, 10, 4, 4, older_ns)); + + manager->set_base_map(internal_new); + + easynav::NavState navstate; + navstate.set("map.base", external_old); + + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); + + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); + EXPECT_EQ(dyn_ptr->getCost(3, 3), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getCost(4, 4), easynav::FREE_SPACE); +} + +TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) +{ + auto node = std::make_shared("test_node_incoming"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + + const std::string topic = node->get_name() + std::string("/test/incoming_map"); + auto pub = node->create_publisher( + topic, rclcpp::QoS(1).transient_local().reliable()); + pub->on_activate(); + + auto grid = make_grid(10, 10, 5, 5, 123LL * 1000000000LL); + pub->publish(grid); + + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor.spin_some(); + + easynav::NavState navstate; + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getCost(5, 5), easynav::LETHAL_OBSTACLE); + EXPECT_GT(base_after.getLastModifiedStamp().nanoseconds(), 0); + + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); + EXPECT_EQ( + dyn_ptr->getLastModifiedStamp().nanoseconds(), + base_after.getLastModifiedStamp().nanoseconds()); +} + ///// \brief Dynamic map update test with point cloud //TEST_F(CostmapMapsManagerTest, BasicDynamicUpdate) //{ @@ -55,8 +203,8 @@ class CostmapMapsManagerTest : public ::testing::Test // auto manager = std::make_shared(); // manager->initialize(node, "test"); // -// easynav::Costmap2D static_map(30, 30, 0.1, -1.5, -1.5); -// manager->set_static_map(static_map); +// easynav::Costmap2D base_map(30, 30, 0.1, -1.5, -1.5); +// manager->set_base_map(base_map); // // easynav::NavState navstate; // auto perception = std::make_shared(); @@ -77,8 +225,8 @@ class CostmapMapsManagerTest : public ::testing::Test // // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.dynamic")); -// const auto & map = navstate.get("map.dynamic"); +// ASSERT_TRUE(navstate.has("map")); +// const auto & map = navstate.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); @@ -120,8 +268,8 @@ class CostmapMapsManagerTest : public ::testing::Test // easynav::NavState navstate; // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.static")); -// const auto & map = navstate.get("map.static"); +// ASSERT_TRUE(navstate.has("map.base")); +// const auto & map = navstate.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); @@ -141,17 +289,17 @@ class CostmapMapsManagerTest : public ::testing::Test // auto manager = std::make_shared(); // manager->initialize(node, "test_savemap"); // -// // Create a 200x200 static costmap with resolution 0.05 and origin at (0, 0) +// // Create a 200x200 base costmap with resolution 0.05 and origin at (0, 0) // const unsigned int width = 200; // const unsigned int height = 200; -// easynav::Costmap2D map_static(width, height, 0.05, 0.0, 0.0); +// easynav::Costmap2D map_base(width, height, 0.05, 0.0, 0.0); // // // Set a vertical line of lethal obstacles at x = 30 // for (unsigned int y = 0; y < height; ++y) { -// map_static.setCost(30, y, easynav::LETHAL_OBSTACLE); +// map_base.setCost(30, y, easynav::LETHAL_OBSTACLE); // } // -// manager->set_static_map(map_static); +// manager->set_base_map(map_base); // // const std::string yaml_path = "/tmp/savemap_test_map"; // const std::string service_name = "/test_savemap_node/test_savemap/savemap"; @@ -181,11 +329,11 @@ class CostmapMapsManagerTest : public ::testing::Test // easynav::Costmap2D loaded_map(loaded_grid); // // // Check map dimensions match -// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_static.getSizeInCellsX()); -// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_static.getSizeInCellsY()); +// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_base.getSizeInCellsX()); +// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_base.getSizeInCellsY()); // -// for (unsigned int y = 0; y < map_static.getSizeInCellsY(); ++y) { -// for (unsigned int x = 0; x < map_static.getSizeInCellsX(); ++x) { +// for (unsigned int y = 0; y < map_base.getSizeInCellsY(); ++y) { +// for (unsigned int x = 0; x < map_base.getSizeInCellsX(); ++x) { // if (x == 30) { // EXPECT_EQ(loaded_map.getCost(x, y), easynav::LETHAL_OBSTACLE) // << "Expected LETHAL_OBSTACLE at (" << x << "," << y << ")"; diff --git a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst index 0e2272bd..3a38c20b 100644 --- a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package easynav_navmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Update calls to deprecated get_package_share_directory +* Navstate key among filters are always map, not an arbitrary key +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index 09006d4a..42b24953 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -5,15 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -22,6 +22,7 @@ find_package(ament_index_cpp REQUIRED) find_package(navmap_core REQUIRED) find_package(navmap_ros REQUIRED) find_package(navmap_ros_interfaces REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(std_srvs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) @@ -44,12 +45,16 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + Eigen3::Eigen tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib ament_index_cpp::ament_index_cpp navmap_core::navmap_core navmap_ros::navmap_ros + easynav_sensors::easynav_sensors ${sensor_msgs_TARGETS} ${tf2_geometry_msgs_TARGETS} ${navmap_ros_interfaces_TARGETS} @@ -90,12 +95,15 @@ ament_export_targets(export_${PROJECT_NAME}) # Register the plugin pluginlib_export_plugin_description_file(easynav_core easynav_navmap_maps_manager_plugins.xml) -pluginlib_export_plugin_description_file(easynav_navmap_filter easynav_navmap_maps_manager_obstacle_filter.xml) -pluginlib_export_plugin_description_file(easynav_navmap_filter easynav_navmap_maps_manager_inflation_filter.xml) +pluginlib_export_plugin_description_file(easynav_navmap_maps_manager easynav_navmap_maps_manager_obstacle_filter.xml) +pluginlib_export_plugin_description_file(easynav_navmap_maps_manager easynav_navmap_maps_manager_inflation_filter.xml) ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle + eigen tf2 tf2_ros pluginlib @@ -104,6 +112,7 @@ ament_export_dependencies( navmap_core navmap_ros navmap_ros_interfaces + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 1f7ce673..f019a211 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -1,66 +1,135 @@ # easynav_navmap_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains a [NavMap](https://github.com/EasyNavigation/NavMap) (triangulated 3D surface) and publishes full maps and layer updates; supports importing from YAML OccupancyGrid or point clouds. +This package also includes map filters implemented as plugins (`ObstacleFilter` and `InflationFilter`) that operate on NavMap layers to detect obstacles and inflate their costs, enabling cost-aware path planning. + ## Authors and Maintainers -- **Authors:** Intelligent Robotics Lab -- **Maintainers:** Francisco Martín Rico + +- **Authors:** Intelligent Robotics Lab +- **Maintainer:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager` -- **Type:** `easynav::NavMapMapsManager` +- **Type:** `easynav::navmap::NavMapMapsManager` - **Base Class:** `easynav::MapsManagerBase` - **Library:** `easynav_navmap_maps_manager` -- **Description:** Maps Manager that maintains a NavMap (triangulated 3D surface) and publishes full maps and layer updates; supports importing from YAML OccupancyGrid or point clouds. +- **Description:** + Maps Manager that maintains a NavMap (triangulated 3D surface) and publishes full maps and layer updates; supports importing from YAML OccupancyGrid or point clouds, and applying dynamic filters to generate obstacle and inflated layers. ## Parameters -All parameters are declared under the plugin namespace, i.e., `//easynav_navmap_maps_manager/NavMapMapsManager/...`. + +All parameters are declared under the plugin namespace, i.e. +`//easynav_navmap_maps_manager/NavMapMapsManager/...` | Name | Type | Default | Description | |---|---|---:|---| -| `.package` | `string` | `""` | Package name to resolve relative paths via ament index. | +| `.package` | `string` | `""` | Package name to resolve relative paths via `ament_index`. | +| `.freq` | `double` | `10.0` | Update frequency (Hz) for map publishing and filter execution. | +| `.navmap_path_file` | `string` | `""` | Relative path (inside the package) to a `.navmap` file to load at startup. | | `.occmap_path_file` | `string` | `""` | Relative path (inside the package) to a ROS YAML OccupancyGrid to import as NavMap. | -| `.navmap_path_file` | `string` | `""` | Relative path (inside the package) to a PCD/PLY used to build NavMap. | +| `.pcd_path_file` | `string` | `""` | Relative path (inside the package) to a point cloud (PCD/PLY) used to build a NavMap surface. | +| `.filters` | `list` | `[]` | Ordered list of filter plugin names to be applied after map loading (e.g. `["obstacles", "inflation"]`). | + +### Filter Plugins + +#### **ObstacleFilter** +- **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/ObstacleFilter` +- **Type:** `easynav::navmap::ObstacleFilter` +- **Description:** + Detects occupied NavCels from input point clouds (`points` key in `NavState`) and marks them as `LETHAL_OBSTACLE` in the `"obstacles"` layer. + The filter groups 3D points into voxels and marks cells as occupied if a sufficient vertical structure is detected (either multiple bins along the z-axis or a vertical span exceeding a threshold). + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.vertical_bins_min` | `int` | `3` | Minimum number of vertical bins required to consider a column as an obstacle. | +| `.height_threshold` | `double` | `0.25` | Minimum vertical height (in meters) between max and min z to mark as an obstacle. | +| `.downsample` | `double` | `0.3` | Voxel size used to downsample point clouds before obstacle detection. | +| `.fuse_frame` | `string` | `"map"` | Frame in which points are fused before projection into NavMap. | +| **Input Key:** | | | Reads list of point clouds from `NavState` group `"points"`. | +| **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | + +#### **InflationFilter** + +- **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter` +- **Type:** `easynav::navmap::InflationFilter` +- **Description:** + Expands obstacle information from the `"obstacles"` layer into an `"inflated_obstacles"` layer, assigning graded costs depending on distance to obstacles and map boundaries (NavCels with missing neighbors). + This filter mimics the behavior of `costmap_2d::InflationLayer` but operates on the NavMap triangular mesh. + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.inflation_radius` | `double` | `0.3` | Maximum inflation distance (m) from obstacles. | +| `.cost_scaling_factor` | `double` | `3.0` | Exponential decay rate controlling how fast cost decreases with distance. | +| `.inscribed_radius` | `double` | `0.3` | Radius of inscribed zone (constant high cost before decay). | +| **Input Layer:** | | | Reads from `"obstacles"`. | +| **Output Layer:** | | | Writes to `"inflated_obstacles"`. | ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Publisher | `//map` | `navmap_ros_interfaces/msg/NavMap` | Published full NavMap. | QoS depth=1 | -| Publisher | `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Incremental NavMap layer updates. | QoS depth=100 | -| Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid to import into NavMap. | QoS depth=1, transient_local, reliable | -| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update NavMap. | QoS depth=100 | - +| Publisher | `//map` | `navmap_ros_interfaces/msg/NavMap` | Publishes the full NavMap. | depth=1 | +| Publisher | `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Publishes incremental layer updates. | depth=100 | +| Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid to import into NavMap. | depth=1, transient_local, reliable | +| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update NavMap. | depth=100 | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| -| Service Server | `//savemap` | `std_srvs/srv/Trigger` | Save current NavMap to disk. | - +| Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current NavMap and layers to disk. | ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| -| `map` | `::navmap::NavMap` | **Read** | Existing NavMap from state, if available. | -| `map.navmap` | `::navmap::NavMap` | **Write** | Stores/broadcasts the maintained NavMap. | - +| `map.navmap` | `::navmap::NavMap` | **Read** | Reads the NavMap if already present in NavState. | +| `map.navmap` | `::navmap::NavMap` | **Write** | Stores the maintained NavMap, including generated layers `"obstacles"` and `"inflated_obstacles"`. | ## TF Frames + | Role | Transform | Notes | |---|---|---| -| Publishes | `—` | No TF broadcasting in this manager; outputs are stamped in their configured frame. | - +| Publishes | — | No TF broadcasting in this manager; outputs are stamped in their configured frame. | + +## Example Configuration + +```yaml +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [navmap] + navmap: + freq: 10.0 + plugin: easynav_navmap_maps_manager/NavMapMapsManager + package: easynav_indoor_testcase + navmap_path_file: maps/excavation_urjc.navmap + filters: [obstacles, inflation] + obstacles: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/ObstacleFilter + height_threshold: 0.25 + inflation: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter + inflation_radius: 1.0 + cost_scaling_factor: 2.0 +``` ## License -GPL-3.0-only + +Apache-2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp index c64609bf..ed3b393c 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the NavMapMapsManager method. @@ -24,11 +20,6 @@ #define EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAP_MAPS_MANAGER_HPP_ #include -#include -#include -#include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" @@ -36,17 +27,12 @@ #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" #include "pluginlib/class_loader.hpp" -#include "yaets/tracing.hpp" - namespace easynav { namespace navmap @@ -78,9 +64,9 @@ class NavMapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp index 39881849..70f010b9 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp @@ -1,32 +1,24 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ -#include +#include #include - -#include "pluginlib/class_loader.hpp" - #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -42,8 +34,8 @@ class InflationFilter : public NavMapFilter public: InflationFilter(); - virtual std::expected on_initialize(); - virtual void update(::easynav::NavState & nav_state); + virtual void on_initialize() override; + virtual void update(::easynav::NavState & nav_state) override; bool inflate_layer_u8( ::navmap::NavMap & nm, diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp index 7c57c536..e245681e 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp @@ -1,31 +1,24 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ -#include #include -#include "navmap_core/NavMap.hpp" - +#include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -38,13 +31,12 @@ class NavMapFilter public: NavMapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix = ""); + const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; virtual bool is_adding_layer() {return false;} @@ -53,17 +45,14 @@ class NavMapFilter float get_map_resolution() {return map_resolution_;} void set_map_resolution(float resolution) {map_resolution_ = resolution;} -protected: - std::shared_ptr get_node() const; - const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; +protected: + std::shared_ptr get_node() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; float map_resolution_ {0.1}; }; diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp index ebbd3eb4..ee342bca 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp @@ -1,31 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include -#include "pluginlib/class_loader.hpp" - #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -41,11 +33,11 @@ class ObstacleFilter : public NavMapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); - virtual void update(::easynav::NavState & nav_state); + virtual void on_initialize() override; + virtual void update(::easynav::NavState & nav_state) override; - bool is_adding_layer() override {return true;} - std::string get_layer_name() override {return "obstacles";} + virtual bool is_adding_layer() override {return true;} + virtual std::string get_layer_name() override {return "obstacles";} private: ::navmap::NavMap navmap_; diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp deleted file mode 100644 index 0a69accd..00000000 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#ifndef EASYNAV_NAVMAP_MAPS_UTILS_HPP_ -#define EASYNAV_NAVMAP_MAPS_UTILS_HPP_ - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav -{ -namespace navmap -{ - - -inline void triangle_angles_deg( - const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C, - float & angA, float & angB, float & angC); - -// PCA local para obtener plano tangente en v -// Devuelve dos ejes ortogonales t1, t2 en el plano tangente. -// Si falla PCA (muy pocos puntos), usa un par ortonormal arbitrario. -inline void local_tangent_basis( - const std::vector & nbrs, - Eigen::Vector3f & t1, Eigen::Vector3f & t2); - -// Ordena vecinos por ángulo en el plano tangente de v -inline void sort_neighbors_angular( - const Eigen::Vector3f & vpos, - const std::vector> & nbrs, // (idx, pos) - std::vector & out_ordered); - -// Intenta crear un triángulo (i,j,k) bajo las restricciones -inline bool try_add_triangle( - int i, int j, int k, - const pcl::PointCloud & cloud, - const Params & P, - std::unordered_set & tri_set, - std::unordered_set & edge_set, - std::vector & tris); - - -std::vector grow_surface_from_seed( - const pcl::PointCloud & cloud, - int seed_idx, - const Params & P); -} // namespace navmap -} // namespace easynav -#endif // EASYNAV_NAVMAP_MAPS_UTILS_HPP_ diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6951d89d..570a046c 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -2,15 +2,18 @@ easynav_navmap_maps_manager - 0.0.2 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake easynav_common easynav_core + rclcpp + rclcpp_lifecycle + eigen tf2 tf2_ros tf2_geometry_msgs @@ -21,6 +24,7 @@ navmap_core navmap_ros navmap_ros_interfaces + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index c94e4223..2d15c28a 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -1,30 +1,25 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include +#include #include #include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -45,20 +40,26 @@ NavMapMapsManager::NavMapMapsManager() { ::easynav::NavState::register_printer<::navmap::NavMap>( [](const ::navmap::NavMap & map) { - (void)map; std::ostringstream oss; - oss << "NavMap Navcells = " << map.navcels.size() << "\tlayers = " << - map.list_layers().size(); + oss << "NavMap: navcels=" << map.navcels.size() + << " surfaces=" << map.surfaces.size() + << " layers=" << map.list_layers().size() << " ("; + + for (const auto & layer : map.list_layers()) { + oss << layer << " "; + } + oss << ")"; + return oss.str(); }); - // navmap_filters_loader_ = std::make_unique>( - // "easynav_navmap_filter", "easynav::navmap::NavMapFilter"); + navmap_filters_loader_ = std::make_unique>( + "easynav_navmap_maps_manager", "easynav::navmap::NavMapFilter"); } NavMapMapsManager::~NavMapMapsManager() {} -std::expected +void NavMapMapsManager::on_initialize() { auto node = get_node(); @@ -83,61 +84,66 @@ NavMapMapsManager::on_initialize() node->get_parameter(plugin_name + "." + navmap_filter + ".plugin", plugin); try { - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loading NavMapFilter %s [%s]", navmap_filter.c_str(), plugin.c_str()); - std::shared_ptr instance; instance = navmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + navmap_filter, - get_tf_prefix()); - - if (!result) { - RCLCPP_ERROR(node->get_logger(), - "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - return std::make_unexpected("Unable to initialize " + - plugin + " . Error: " + result.error()); + try { + instance->initialize(node, plugin_name + "." + navmap_filter); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); + throw; } navmap_filters_.push_back(instance); - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loaded NavMapFilter %s [%s]", navmap_filter.c_str(), plugin.c_str()); } catch (pluginlib::PluginlibException & ex) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to load plugin easynav::navmap::NavMapFilter. Error: %s", ex.what()); - return std::make_unexpected("Unable to load plugin easynav::navmap::NavMapFilter " + - navmap_filter + " . Error: " + ex.what()); + throw std::runtime_error( + "Unable to load plugin easynav::navmap::NavMapFilter " + + navmap_filter + " . Error: " + ex.what()); } } navmap_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); layer_updates_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map_updates", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map_updates", rclcpp::QoS(100)); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (!package_name.empty() && !occmap_path_file.empty()) { try { const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } nav_msgs::msg::OccupancyGrid occ_msg; if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::make_unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } resolution_ = occ_msg.info.resolution; navmap_ = navmap_ros::from_occupancy_grid(occ_msg); navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } @@ -147,12 +153,12 @@ NavMapMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + navmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (navmap_ros::io::load_from_file(map_path_, navmap_)) { navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } else { @@ -161,36 +167,38 @@ NavMapMapsManager::on_initialize() } incoming_occ_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), - [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { + [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { resolution_ = msg->info.resolution; navmap_ = navmap_ros::from_occupancy_grid(*msg); navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), - [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { + [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { navmap_ros::BuildParams params; navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/savemap", [this]( const std::shared_ptr request, std::shared_ptr response) @@ -200,8 +208,6 @@ NavMapMapsManager::on_initialize() navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); // ToDo }); - - return {}; } void @@ -219,27 +225,17 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) nav_state.set("map.navmap", navmap_); } - nav_state.set("map.navmap", navmap_); + for (const auto & filter : navmap_filters_) { + filter->set_map_resolution(resolution_); + filter->update(nav_state); -// if (!navmap_.has_layer("occupancy")) { -// navmap_.add_layer("occupancy", "Per-NavCel occupancy (0=free, 255=unknown)", "", -// static_cast(0)); -// } -// -// for (const auto & filter : navmap_filters_) { -// filter->set_map_resolution(resolution_); -// filter->update(nav_state); -// } -// -// navmap_ = nav_state.get<::navmap::NavMap>("map"); -// -// for (const auto & filter : navmap_filters_) { -// -// if (filter->is_adding_layer() && navmap_.has_layer(filter->get_layer_name())) { -// auto update_msg = navmap_ros::to_msg(navmap_, filter->get_layer_name()); -// layer_updates_pub_->publish(update_msg); -// } -// } + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); + + if (filter->is_adding_layer() && navmap.has_layer(filter->get_layer_name())) { + auto update_msg = navmap_ros::to_msg(navmap, filter->get_layer_name()); + layer_updates_pub_->publish(update_msg); + } + } } diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 4935d8bb..9ae969f8 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -39,15 +39,13 @@ *********************************************************************/ -#include #include #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/InflationFilter.hpp" @@ -57,17 +55,11 @@ namespace easynav namespace navmap { -static constexpr unsigned char NO_INFORMATION = 255; -static constexpr unsigned char LETHAL_OBSTACLE = 254; -static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr unsigned char MAX_NON_OBSTACLE = 252; -static constexpr unsigned char FREE_SPACE = 0; - InflationFilter::InflationFilter() -: inflation_radius_(0), - cost_scaling_factor_(0) -{ -} +: inflation_radius_(0.0f), + cost_scaling_factor_(0.0f), + inscribed_radius_(0.0f) +{} bool InflationFilter::inflate_layer_u8( ::navmap::NavMap & nm, @@ -77,94 +69,119 @@ bool InflationFilter::inflate_layer_u8( float cost_scaling_factor, float inscribed_radius) { - using namespace ::navmap; + using ::navmap::NavCelId; + using namespace navmap_ros; // bring FREE_SPACE, LETHAL_OBSTACLE, etc. + if (nm.navcels.empty() || inflation_radius <= 0.0f || cost_scaling_factor <= 0.0f) { return false; } - // Capas - auto src_view = std::dynamic_pointer_cast>(nm.layers.get(src_layer)); + // Source layer + auto src_view = + std::dynamic_pointer_cast<::navmap::LayerView>(nm.layers.get(src_layer)); if (!src_view || src_view->size() != nm.navcels.size()) { return false; } - auto dst_view = nm.layers.add_or_get(dst_layer, nm.navcels.size(), - layer_type_tag()); + + // Destination layer (create if missing) + auto dst_view = nm.layers.add_or_get( + dst_layer, nm.navcels.size(), + ::navmap::layer_type_tag()); if (!dst_view) {return false;} if (dst_view->data().size() != nm.navcels.size()) { - const_cast &>(dst_view->data()).assign(nm.navcels.size(), FREE_SPACE); + const_cast &>(dst_view->data()).assign(nm.navcels.size(), FREE_SPACE); } const size_t N = nm.navcels.size(); const float R = inflation_radius; - // const float k = cost_scaling_factor; const float r_ins = std::clamp(inscribed_radius, 0.0f, R); - // Precomputar centroides XY + // Precompute XY centroids for each NavCel std::vector C(N); for (NavCelId cid = 0; cid < N; ++cid) { const Eigen::Vector3f cc = nm.navcel_centroid(cid); C[cid] = {cc.x(), cc.y()}; } - // Distancias y cola (Dijkstra multi-fuente) + // Distance map (multi-source Dijkstra) const float INF = std::numeric_limits::infinity(); std::vector dist(N, INF); struct Node { float d; NavCelId cid; bool operator<(const Node & o) const {return d > o.d;} }; std::priority_queue pq; - // Inicializar dst y semillas - auto & dst = dst_view->mutable_data(); // marca dirty + auto & dst = dst_view->mutable_data(); // mark dirty const auto & src = src_view->data(); + // Initialize destinations and seeds bool any_seed = false; for (NavCelId cid = 0; cid < N; ++cid) { - const uint8_t s = src[cid]; + const std::uint8_t s = src[cid]; + + // Seed 1: lethal obstacles if (s == LETHAL_OBSTACLE) { dist[cid] = 0.0f; pq.push({0.0f, cid}); dst[cid] = LETHAL_OBSTACLE; any_seed = true; - } else if (s == NO_INFORMATION) { - dst[cid] = NO_INFORMATION; // mantener desconocido + continue; + } + + // Seed 2: boundary triangles (missing neighbors) + if (nm.navcel_neighbors(cid).size() < 3) { + dist[cid] = 0.0f; + pq.push({0.0f, cid}); + dst[cid] = LETHAL_OBSTACLE; + any_seed = true; + continue; + } + + // Unknown handling + if (s == NO_INFORMATION) { + dst[cid] = NO_INFORMATION; } else { - // si dst==src (in-place), preserva el valor actual; si no, escribe 0 - if (dst_layer != src_layer) {dst[cid] = FREE_SPACE;} + if (dst_layer != src_layer) { + dst[cid] = FREE_SPACE; + } } } - if (!any_seed) {return true;} // nada que inflar - auto cost_from_dist = [&](float d) -> uint8_t { - if (d <= 0.0f) {return LETHAL_OBSTACLE;} // 254 - if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} // 253 - if (d > R) {return FREE_SPACE;} // 0 + if (!any_seed) { + return true; // nothing to inflate + } + + // Distance-to-cost conversion + auto cost_from_dist = [&](float d) -> std::uint8_t { + if (d <= 0.0f) {return LETHAL_OBSTACLE;} + if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} + if (d > R) {return FREE_SPACE;} - const float x = d - r_ins; // >= 0 - const double factor = -1.0 * cost_scaling_factor * x; - double c = std::exp(factor) * (static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1); + const float x = d - r_ins; + const double base = static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1; + double c = std::exp(-cost_scaling_factor * static_cast(x)) * base; if (c < 0.0) {c = 0.0;} - if (c > INSCRIBED_INFLATED_OBSTACLE - 1) {c = INSCRIBED_INFLATED_OBSTACLE - 1;} - return static_cast(std::lround(c)); + if (c > base) {c = base;} + return static_cast(std::lround(c)); }; - // Dijkstra acotado por R + // Bounded Dijkstra expansion while (!pq.empty()) { const auto [du, u] = pq.top(); pq.pop(); if (du != dist[u]) {continue;} if (du > R) {continue;} - // Escribir coste si no es desconocido ni letal if (dst[u] != NO_INFORMATION && dst[u] != LETHAL_OBSTACLE) { - const uint8_t c = cost_from_dist(du); - if (c > dst[u]) {dst[u] = c;} // tomamos el máximo (monótono) + const std::uint8_t c = cost_from_dist(du); + if (c > dst[u]) {dst[u] = c;} // monotonic accumulation } - // Relajar vecinos + // Neighbor relaxation for (NavCelId v : nm.navcel_neighbors(u)) { const size_t vidx = static_cast(v); if (vidx >= N) {continue;} - // Si quieres que el desconocido BLOQUEE la propagación, descomenta: - // if (src[v] == NO_INFORMATION) continue; + if (src[v] == NO_INFORMATION) { + continue; + } const float step = (C[u] - C[v]).norm(); if (step <= 0.0f) {continue;} @@ -179,49 +196,56 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); - inflation_radius_ = 0.3; - cost_scaling_factor_ = 3.0; - inscribed_radius_ = 0.3; + // Defaults; may be overridden in parameters + inflation_radius_ = 0.30f; + cost_scaling_factor_ = 3.0f; + inscribed_radius_ = 0.30f; node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); - RCLCPP_INFO(node->get_logger(), - "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", - inflation_radius_, cost_scaling_factor_); - - return {}; + RCLCPP_INFO( + node->get_logger(), + "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", + inflation_radius_, cost_scaling_factor_, inscribed_radius_); } -void -InflationFilter::update(::easynav::NavState & nav_state) +void InflationFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map")) { + if (!nav_state.has("map.navmap")) { return; } - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + + const bool ok = inflate_layer_u8( + navmap_, + "obstacles", + "inflated_obstacles", + inflation_radius_, + cost_scaling_factor_, + inscribed_radius_); - if (!inflate_layer_u8(navmap_, "obstacles", "inflated_obstacles", - inflation_radius_, cost_scaling_factor_, 0.3)) - { - RCLCPP_ERROR(parent_node_->get_logger(), "Error inflating at ObstacleFilter"); + if (!ok) { + RCLCPP_ERROR(parent_node_->get_logger(), "InflationFilter: inflate_layer_u8() failed"); + return; } - nav_state.set("map", navmap_); + nav_state.set("map.navmap", navmap_); } - } // namespace navmap } // namespace easynav + #include PLUGINLIB_EXPORT_CLASS(easynav::navmap::InflationFilter, easynav::navmap::NavMapFilter) diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp index a850edd7..47e9a95e 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp @@ -1,29 +1,20 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include -#include "navmap_core/NavMap.hpp" -#include "easynav_common/types/NavState.hpp" - #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" namespace easynav @@ -36,18 +27,15 @@ NavMapFilter::NavMapFilter() } -std::expected +void NavMapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; - return on_initialize(); + on_initialize(); } std::shared_ptr @@ -62,11 +50,5 @@ NavMapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -NavMapFilter::get_tf_prefix() const -{ - return tf_prefix_; -} - } // namespace navmap } // namespace easynav diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index faec9835..0b1fad3f 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -1,31 +1,27 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include +#include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" @@ -40,54 +36,130 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} -void -ObstacleFilter::update(::easynav::NavState & nav_state) +void ObstacleFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map")) { - return; - } - if (!nav_state.has("points")) { + if (!nav_state.has("map.navmap")) {return;} + + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (!navmap_.layer_copy("occupancy", "obstacles")) { - RCLCPP_ERROR(parent_node_->get_logger(), "Error copying layers at ObstacleFilter"); - return; - } + navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(get_map_resolution()) - .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + const auto & points = PointPerceptionsOpsView(perceptions) + .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) + .downsample(0.3) + .fuse(tf_info.map_frame) .as_points(); - size_t sidx = 0; - for (const auto & p : fused) { - if (std::isnan(p.x)) {continue;} - ::navmap::NavCelId cid; - Eigen::Vector3f bary; - Eigen::Vector3f hit; + const float voxel_xy = 0.30f; + const float voxel_z = 0.20f; + + struct Accum + { + std::unordered_set z_bins; + float max_z = -std::numeric_limits::infinity(); + float min_z = std::numeric_limits::infinity(); + }; + + struct Key + { + int ix, iy; + bool operator==(const Key & o) const noexcept {return ix == o.ix && iy == o.iy;} + }; + struct KeyHash + { + std::size_t operator()(const Key & k) const noexcept + { + std::size_t h1 = std::hash{}(static_cast(k.ix)); + std::size_t h2 = std::hash{}(static_cast(k.iy)); + return h1 ^ (h2 + 0x9e3779b97f4a7c15ULL + (h1 << 6) + (h1 >> 2)); + } + }; + + std::unordered_map bins; + bins.reserve(points.size() / 4 + 1); + + for (const auto & p : points.points) { + const float x = p.x; + const float y = p.y; + const float z = p.z; - if (navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit)) { - uint8_t v = navmap_.layer_get("occupancy", cid, 255); - if (v == 0) { - navmap_.set_area(hit, 254, "obstacles", ::navmap::AreaShape::CIRCULAR, 0.1); + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {continue;} + + const int ix = static_cast(std::floor(x / voxel_xy)); + const int iy = static_cast(std::floor(y / voxel_xy)); + const int iz = static_cast(std::floor(z / voxel_z)); + + auto & acc = bins[{ix, iy}]; + acc.z_bins.insert(iz); + if (z > acc.max_z) {acc.max_z = z;} + if (z < acc.min_z) {acc.min_z = z;} + } + + std::optional last_surface; + std::optional<::navmap::NavCelId> last_cid; + + const float height_threshold = 0.25f; + + for (const auto & kv : bins) { + const auto & key = kv.first; + const auto & acc = kv.second; + + const float cx = (static_cast(key.ix) + 0.5f) * voxel_xy; + const float cy = (static_cast(key.iy) + 0.5f) * voxel_xy; + const float dz = acc.max_z - acc.min_z; + + if (acc.z_bins.size() <= 2 && dz <= height_threshold) { + continue; + } + + const float cz = acc.max_z; + Eigen::Vector3f query(cx, cy, cz); + + size_t surface_idx = 0; + ::navmap::NavCelId cid; + Eigen::Vector3f bary, hit; + + ::navmap::NavMap::LocateOpts opts; + opts.use_downward_ray = true; + opts.height_eps = 0.50f; + if (last_surface) {opts.hint_surface = *last_surface;} + if (last_cid) {opts.hint_cid = *last_cid;} + + bool ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit, opts); + + if (!ok) { + ::navmap::NavMap::LocateOpts nohint; + nohint.use_downward_ray = true; + nohint.height_eps = 0.50f; + ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit, nohint); + if (!ok) { + ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit); } } + + if (ok) { + navmap_.layer_set( + get_layer_name(), cid, static_cast(navmap_ros::LETHAL_OBSTACLE)); + last_surface = surface_idx; + last_cid = cid; + } } - nav_state.set("map", navmap_); + nav_state.set("map.navmap", navmap_); } + } // namespace navmap } // namespace easynav #include diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp deleted file mode 100644 index 30b07dc3..00000000 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#include "easynav_navmap_maps_manager/utils.hpp" - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav -{ -namespace navmap -{ - -// PCA local para obtener plano tangente en v -// Devuelve dos ejes ortogonales t1, t2 en el plano tangente. -// Si falla PCA (muy pocos puntos), usa un par ortonormal arbitrario. -inline void local_tangent_basis( - const std::vector & nbrs, - Eigen::Vector3f & t1, Eigen::Vector3f & t2) -{ - if (nbrs.size() < 3) { - // base por defecto (proyección estable) - t1 = Eigen::Vector3f::UnitX(); - t2 = Eigen::Vector3f::UnitY(); - return; - } - - // Centra - Eigen::Vector3f mean = Eigen::Vector3f::Zero(); - for (auto & p : nbrs) { - mean += p; - } - mean /= static_cast(nbrs.size()); - - // Covarianza - Eigen::Matrix3f C = Eigen::Matrix3f::Zero(); - for (auto & p : nbrs) { - Eigen::Vector3f d = p - mean; - C += d * d.transpose(); - } - C /= static_cast(nbrs.size()); - - // Autovectores - Eigen::SelfAdjointEigenSolver es(C); - // Valores ascendentes: 0 -> menor (normal aproximada) - // Eigen::Vector3f e0 = es.eigenvectors().col(0); // normal aproximada - Eigen::Vector3f e1 = es.eigenvectors().col(1); - Eigen::Vector3f e2 = es.eigenvectors().col(2); - - // Plano tangente: proyectaremos en {e1, e2} - // Asegura ortonormalidad y evita inversión esporádica - t1 = e1.normalized(); - t2 = (e2 - e2.dot(t1) * t1).normalized(); -} - -inline void triangle_angles_deg( - const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C, - float & angA, float & angB, float & angC) -{ - // Lados opuestos a A, B, C - float a = (B - C).norm(); - float b = (C - A).norm(); - float c = (A - B).norm(); - - // Evitar divisiones por cero - const float eps = 1e-12f; - a = std::max(a, eps); - b = std::max(b, eps); - c = std::max(c, eps); - - // Ley de cosenos con clamp numérico - auto angle_from = [](float opp, float x, float y) -> float { - float cosv = (x * x + y * y - opp * opp) / (2.0f * x * y); - cosv = std::min(1.0f, std::max(-1.0f, cosv)); - return std::acos(cosv) * 180.0f / static_cast(M_PI); - }; - - angA = angle_from(a, b, c); - angB = angle_from(b, c, a); - angC = angle_from(c, a, b); -} - -// Ordena vecinos por ángulo en el plano tangente de v -inline void sort_neighbors_angular( - const Eigen::Vector3f & vpos, - const std::vector> & nbrs, // (idx, pos) - std::vector & out_ordered) -{ - std::vector pts; - pts.reserve(nbrs.size() + 1); - pts.push_back(vpos); - for (auto & it : nbrs) { - pts.push_back(it.second); - } - - Eigen::Vector3f t1, t2; - local_tangent_basis(pts, t1, t2); - - std::vector> ang_idx; - ang_idx.reserve(nbrs.size()); - for (auto & it : nbrs) { - Eigen::Vector3f d = it.second - vpos; - float x = d.dot(t1); - float y = d.dot(t2); - float ang = std::atan2(y, x); // -pi..pi - ang_idx.emplace_back(ang, it.first); - } - - std::sort(ang_idx.begin(), ang_idx.end(), - [](auto & a, auto & b){return a.first < b.first;}); - - out_ordered.clear(); - out_ordered.reserve(ang_idx.size()); - for (auto & ai : ang_idx) { - out_ordered.push_back(ai.second); - } -} - -// Intenta crear un triángulo (i,j,k) bajo las restricciones -inline bool try_add_triangle( - int i, int j, int k, - const pcl::PointCloud & cloud, - const Params & P, - std::unordered_set & tri_set, - std::unordered_set & edge_set, - std::vector & tris) -{ - TriKey tk = make_tri(i, j, k); - if (tri_set.find(tk) != tri_set.end()) {return false;} - - const auto & A = cloud[i]; - const auto & B = cloud[j]; - const auto & C = cloud[k]; - if (!pcl::isFinite(A) || !pcl::isFinite(B) || !pcl::isFinite(C)) {return false;} - - // Longitudes máximas de arista - auto dAB = dist3(A, B); - auto dBC = dist3(B, C); - auto dCA = dist3(C, A); - if (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len) {return false;} - - Eigen::Vector3f a(A.x, A.y, A.z); - Eigen::Vector3f b(B.x, B.y, B.z); - Eigen::Vector3f c(C.x, C.y, C.z); - - // Área mínima - if (tri_area(a, b, c) < P.min_area) {return false;} - - // Pendiente máxima - float slope = tri_slope_deg(a, b, c); - if (slope > P.max_slope_deg) {return false;} - - // Ángulo mínimo en los tres vértices - float angA, angB, angC; - triangle_angles_deg(a, b, c, angA, angB, angC); - if (angA < P.min_angle_deg || angB < P.min_angle_deg || angC < P.min_angle_deg) { - return false; - } - - { - const Eigen::Vector3f up = Eigen::Vector3f::UnitZ(); - Eigen::Vector3f n = (b - a).cross(c - a); - if (n.norm() < 1e-9f) {return false;} - if (n.dot(up) < 0.0f) { - std::swap(j, k); - // si más abajo usas b/c otra vez, re-asigna: - // b = Eigen::Vector3f(cloud[j].x, cloud[j].y, cloud[j].z); - // c = Eigen::Vector3f(cloud[k].x, cloud[k].y, cloud[k].z); - } - } - - // Aceptar - tri_set.insert(tk); - edge_set.insert(make_edge(i, j)); - edge_set.insert(make_edge(j, k)); - edge_set.insert(make_edge(k, i)); - tris.emplace_back(Triangle(i, j, k)); - return true; -} - - -} // namespace navmap -} // namespace easynav diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index c4e1438f..913234a8 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp @@ -1,26 +1,28 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include -#include "navmap_core/NavMap.hpp" - -#include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" -#include "easynav_navmap_maps_manager/map_io.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_srvs/srv/trigger.hpp" #include -#include /// \brief Fixture for NavMapMapsManager tests class NavMapMapsManagerTest : public ::testing::Test @@ -29,17 +31,6 @@ class NavMapMapsManagerTest : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - ::easynav::NavState::register_printer( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override @@ -77,8 +68,8 @@ class NavMapMapsManagerTest : public ::testing::Test // // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.dynamic")); -// const auto & map = ::easynav::NavState.get("map.dynamic"); +// ASSERT_TRUE(::easynav::NavState.has("map")); +// const auto & map = ::easynav::NavState.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); @@ -120,8 +111,8 @@ class NavMapMapsManagerTest : public ::testing::Test // ::easynav::NavState ::easynav::NavState; // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.static")); -// const auto & map = ::easynav::NavState.get("map.static"); +// ASSERT_TRUE(::easynav::NavState.has("map.base")); +// const auto & map = ::easynav::NavState.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp b/maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp deleted file mode 100644 index 5d17909b..00000000 --- a/maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp +++ /dev/null @@ -1,205 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. - -#include - -#include "easynav_navmap_maps_manager/utils.hpp" - -/// \brief Fixture for NavMapMapsManager tests -class NavMapMapsManagerUtilsTests : public ::testing::Test -{ -protected: - void SetUp() override - { - } - - void TearDown() override - { - } -}; - -TEST_F(NavMapMapsManagerUtilsTests, BasicUsage) -{ - easynav::navmap::Params P; - P.max_edge_len = 2.4f; // algo > resolución del downsample - P.max_slope_deg = 25.0f; // ajusta según “transitabilidad” - P.neighbor_radius = 2.4f; // radio razonable - P.use_radius = true; - - pcl::PointCloud input_points; - - input_points.push_back({0.5, 0.5, 0.0}); - input_points.push_back({1.9, 0.5, 0.0}); - input_points.push_back({0.5, 1.5, 0.0}); - input_points.push_back({-0.5, 1.5, 0.0}); - - int seed = 0; - auto triangles = easynav::navmap::grow_surface_from_seed(input_points, seed, P); - - - std::cerr << "Triángulos: " << triangles.size() << std::endl; - for (size_t i = 0; i < triangles.size(); i++) { - pcl::PointXYZ v0 = input_points[triangles[i][0]]; - pcl::PointXYZ v1 = input_points[triangles[i][1]]; - pcl::PointXYZ v2 = input_points[triangles[i][2]]; - - - std::cerr << i << "\t(" << v0.x << ", " << v0.y << ", " << v0.z << ")" << - "\t(" << v1.x << ", " << v1.y << ", " << v1.z << ")" << - "\t(" << v2.x << ", " << v2.y << ", " << v2.z << ")" << std::endl; - } - -} - - -///// \brief Dynamic map update test with point cloud -//TEST_F(NavMapMapsManagerTest, BasicDynamicUpdate) -//{ -// auto node = std::make_shared("test_node"); -// auto manager = std::make_shared(); -// manager->initialize(node, "test"); -// -// easynav::NavMap2D static_map(30, 30, 0.1, -1.5, -1.5); -// manager->set_static_map(static_map); -// -// ::easynav::NavState ::easynav::NavState; -// auto perception = std::make_shared(); -// perception->data.points.resize(2); -// perception->data.points[0].x = 1.0; -// perception->data.points[0].y = 1.0; -// perception->data.points[0].z = 0.2; -// perception->data.points[1].x = -1.0; -// perception->data.points[1].y = -1.0; -// perception->data.points[1].z = 0.2; -// perception->frame_id = "map"; -// perception->stamp = node->now(); -// perception->valid = true; -// -// easynav::PointPerceptions perceptions; -// perceptions.push_back(perception); -// ::easynav::NavState.set("points", perceptions); -// -// manager->update(::easynav::NavState); -// -// ASSERT_TRUE(::easynav::NavState.has("map.dynamic")); -// const auto & map = ::easynav::NavState.get("map.dynamic"); -// -// unsigned int cx, cy; -// ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); -// EXPECT_EQ(map.getCost(cx, cy), easynav::LETHAL_OBSTACLE); -// -// ASSERT_TRUE(map.worldToMap(-1.0, -1.0, cx, cy)); -// EXPECT_EQ(map.getCost(cx, cy), easynav::LETHAL_OBSTACLE); -//} -// -///// \brief OccupancyGrid updates map via subscription -//TEST_F(NavMapMapsManagerTest, IncomingOccupancyGridUpdatesMaps) -//{ -// auto node = std::make_shared("test_node2"); -// auto manager = std::make_shared(); -// manager->initialize(node, "test2"); -// -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(node->get_node_base_interface()); -// -// auto pub = node->create_publisher( -// "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); -// -// pub->on_activate(); -// -// nav_msgs::msg::OccupancyGrid grid; -// grid.header.frame_id = "map"; -// grid.info.width = 10; -// grid.info.height = 10; -// grid.info.resolution = 0.2; -// grid.info.origin.position.x = -1.0; -// grid.info.origin.position.y = -0.6; -// grid.data.assign(100, 0); -// grid.data[55] = 100; -// -// pub->publish(grid); -// executor.spin_some(); -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// -// ::easynav::NavState ::easynav::NavState; -// manager->update(::easynav::NavState); -// -// ASSERT_TRUE(::easynav::NavState.has("map.static")); -// const auto & map = ::easynav::NavState.get("map.static"); -// -// EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); -// EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); -//} -// -///// \brief Helper subclass to force map path for savemap -//class FriendNavMapMapsManager : public easynav::navmap::NavMapMapsManager -//{ -//public: -// void force_path(const std::string & path) {map_path_ = path;} -//}; -// -///// \brief Test that the savemap service correctly stores YAML and PGM files -//TEST_F(NavMapMapsManagerTest, SavemapServiceWorks) -//{ -// auto node = std::make_shared("test_savemap_node"); -// auto manager = std::make_shared(); -// manager->initialize(node, "test_savemap"); -// -// // Create a 200x200 static ::navmap::NavMap with resolution 0.05 and origin at (0, 0) -// const unsigned int width = 200; -// const unsigned int height = 200; -// easynav::NavMap2D map_static(width, height, 0.05, 0.0, 0.0); -// -// // Set a vertical line of lethal obstacles at x = 30 -// for (unsigned int y = 0; y < height; ++y) { -// map_static.setCost(30, y, easynav::LETHAL_OBSTACLE); -// } -// -// manager->set_static_map(map_static); -// -// const std::string yaml_path = "/tmp/savemap_test_map"; -// const std::string service_name = "/test_savemap_node/test_savemap/savemap"; -// -// manager->force_path(yaml_path); -// -// // Create executor and service client -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(node->get_node_base_interface()); -// -// auto client = node->create_client(service_name); -// ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); -// -// // Call savemap service -// auto request = std::make_shared(); -// auto future = client->async_send_request(request); -// executor.spin_until_future_complete(future); -// -// auto response = future.get(); -// EXPECT_TRUE(response->success); -// EXPECT_NE(response->message.find("saved"), std::string::npos); -// -// // Reload the map from the generated YAML + PGM files -// nav_msgs::msg::OccupancyGrid loaded_grid; -// EXPECT_EQ(easynav::loadMapFromYaml(yaml_path + ".yaml", loaded_grid), easynav::LOAD_MAP_SUCCESS); -// -// easynav::NavMap2D loaded_map(loaded_grid); -// -// // Check map dimensions match -// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_static.getSizeInCellsX()); -// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_static.getSizeInCellsY()); -// -// for (unsigned int y = 0; y < map_static.getSizeInCellsY(); ++y) { -// for (unsigned int x = 0; x < map_static.getSizeInCellsX(); ++x) { -// if (x == 30) { -// EXPECT_EQ(loaded_map.getCost(x, y), easynav::LETHAL_OBSTACLE) -// << "Expected LETHAL_OBSTACLE at (" << x << "," << y << ")"; -// } else { -// EXPECT_EQ(loaded_map.getCost(x, y), easynav::FREE_SPACE) -// << "Expected FREE_SPACE at (" << x << "," << y << ")"; -// } -// } -// } -//} diff --git a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst index d30661e8..ef361075 100644 --- a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package easynav_octomap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Update calls to deprecated get_package_share_directory +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt index ea292b81..f59f8814 100644 --- a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt @@ -5,15 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -47,6 +46,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -96,6 +97,8 @@ pluginlib_export_plugin_description_file(easynav_core easynav_octomap_maps_manag ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2 tf2_ros pluginlib diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 700ce944..119b30f2 100644 --- a/maps_managers/easynav_octomap_maps_manager/README.md +++ b/maps_managers/easynav_octomap_maps_manager/README.md @@ -1,21 +1,23 @@ # easynav_octomap_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains an [OctoMap](https://octomap.github.io/) (probabilistic 3D occupancy tree), supports filter plugins (e.g., inflation and obstacle filters), and exposes the map through ROS 2 topics. This README documents **plugin-level parameters only**. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_octomap_maps_manager/OctomapMapsManager` - **Type:** `easynav::octomap::OctomapMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -26,19 +28,16 @@ Maps Manager that maintains an [OctoMap](https://octomap.github.io/) (probabilis ## Parameters -### Plugin Parameters (namespace: `//easynav_octomap_maps_manager/OctomapMapsManager/...`) -| Name | Type | Default | Description | -|---|---|---:|---| -| `.package` | `string` | `""` | Package name used to resolve relative map paths via `ament_index`. | -| `.occmap_path_file` | `string` | `""` | Relative path (inside the package) to a ROS YAML occupancy grid to import as OctoMap. | -| `.octomap_path_file` | `string` | `""` | Relative path (inside the package) to a PCD/PLY/OT file used to build/load the OctoMap. | -| `.filters` | `string[]` | `[]` | List of filter identifiers to be instantiated (see “Filter Parameters”). | -| `..plugin` | `string` | `""` | Type of filter plugin (e.g., `easynav_octomap_maps_manager/InflationFilter`). | +### Plugin Parameters + +The current code (OctomapMapsManager.cpp) has the high-level parameter declarations for package and map paths commented out. Active runtime parameters are therefore limited to filter plugin namespaces populated by entries in `filters` (if/when enabled). Until the declarations are restored, only filter-local parameters are honored. ### Filter Parameters + Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any specific parameters. #### InflationFilter + | Name | Type | Default | Description | |---|---|---:|---| | `.inflation.inflation_radius` | `double` | `0.3` | Inflation radius (m) used to expand occupied cells. | @@ -46,9 +45,10 @@ Each entry in `.filters` defines a sub-namespace `.` wit | `.inflation.inscribed_radius` | `double` | `0.3` | Inscribed radius (m) used by the inflation model. | #### ObstacleFilter + This filter does not declare additional ROS parameters apart from `plugin`. -**Example Configuration** +## Example Configuration ```yaml maps_manager_node: @@ -73,12 +73,14 @@ maps_manager_node: ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Publisher | `//map` | `octomap_msgs/msg/Octomap` | Publishes the current OctoMap. | `depth=1` | -| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud used to build/update the OctoMap. | `depth=100` | +| Publisher | `//map` | `octomap_msgs/msg/Octomap` | Publishes the current OctoMap. | depth=1 | +| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud used to build/update the OctoMap. | depth=100 | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current OctoMap to disk. | @@ -86,13 +88,15 @@ maps_manager_node: --- ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| -| `map` | `::octomap::Octomap` | **Read** | If present in NavState, used as input/seed map. This plugin does not write a NavState key. | +| `map` | `::octomap::Octomap` | **Read** | If present in NavState, used as an input/seed map. (Plugin currently does not write back to NavState.) | --- ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | — | This manager does not broadcast TF; outputs are stamped using the configured frame(s) in the map data. | @@ -100,4 +104,5 @@ maps_manager_node: --- ## License -GPL-3.0-only + +Apache-2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp index 4654a14d..8d84e1b1 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the OctomapMapsManager method. @@ -24,28 +20,18 @@ #define EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAP_MAPS_MANAGER_HPP_ #include -#include -#include -#include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "octomap_msgs/msg/octomap.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "octomap/octomap.h" #include "easynav_octomap_maps_manager/filters/OctomapFilter.hpp" #include "pluginlib/class_loader.hpp" -#include "yaets/tracing.hpp" - namespace easynav { namespace octomap @@ -77,9 +63,9 @@ class OctomapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp index a2d42a7e..f3477aa0 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp @@ -1,30 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ -#include #include - #include "pluginlib/class_loader.hpp" #include "octomap_core/Octomap.hpp" @@ -42,7 +35,7 @@ class InflationFilter : public OctomapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(::easynav::NavState & nav_state); bool inflate_layer_u8( diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp index 91d57b7f..e43778d1 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp @@ -1,27 +1,21 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include #include "pluginlib/class_loader.hpp" @@ -41,7 +35,7 @@ class ObstacleFilter : public OctomapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(::easynav::NavState & nav_state); bool is_adding_layer() override {return true;} diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp index fd6d4df0..340859aa 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp @@ -1,31 +1,25 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ -#include #include #include "octomap/octomap.h" - +#include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -38,13 +32,12 @@ class OctomapFilter public: OctomapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix = ""); + const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; float get_map_resolution() {return map_resolution_;} @@ -55,12 +48,9 @@ class OctomapFilter const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; - protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; float map_resolution_ {0.1}; }; diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp deleted file mode 100644 index ca22b0fb..00000000 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER_UTILS_HPP_ -#define EASYNAV_OCTOMAP_MAPS_MANAGER_UTILS_HPP_ - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav -{ -namespace octomap -{ - - -} // namespace octomap -} // namespace easynav -#endif // EASYNAV_OCTOMAP_MAPS_MANAGER_UTILS_HPP_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..08ebd1c5 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -2,15 +2,17 @@ easynav_octomap_maps_manager - 0.0.2 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2 tf2_ros tf2_geometry_msgs @@ -18,7 +20,6 @@ nav_msgs sensor_msgs ament_index_cpp - octomap octomap_ros octomap_msgs pcl_conversions diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp index bb2ce6d3..1379bb7b 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp @@ -1,29 +1,22 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include #include "easynav_octomap_maps_manager/OctomapMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -62,7 +55,7 @@ OctomapMapsManager::OctomapMapsManager() OctomapMapsManager::~OctomapMapsManager() {} -std::expected +void OctomapMapsManager::on_initialize() { auto node = get_node(); @@ -98,7 +91,7 @@ OctomapMapsManager::on_initialize() // if (!result) { // RCLCPP_ERROR(node->get_logger(), // "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - // return std::make_unexpected("Unable to initialize " + + // throw std::runtime_error("Unable to initialize " + // plugin + " . Error: " + result.error()); // } // @@ -109,27 +102,27 @@ OctomapMapsManager::on_initialize() // } catch (pluginlib::PluginlibException & ex) { // RCLCPP_ERROR(node->get_logger(), // "Unable to load plugin easynav::octomap::OctomapFilter. Error: %s", ex.what()); - // return std::make_unexpected("Unable to load plugin easynav::octomap::OctomapFilter " + + // throw std::runtime_error("Unable to load plugin easynav::octomap::OctomapFilter " + // octomap_filter + " . Error: " + ex.what()); // } // } octomap_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); // if (!package_name.empty() && !occmap_path_file.empty()) { // try { -// const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); +// const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { -// return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); +// throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); // } // // nav_msgs::msg::OccupancyGrid occ_msg; // if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { // std::cerr << "loadMapFromYaml returned" << ret << std::endl; -// return std::make_unexpected("YAML file [" + map_path_ + "] not found or invalid: "); +// throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); // } // // resolution_ = occ_msg.info.resolution; @@ -143,10 +136,10 @@ OctomapMapsManager::on_initialize() // // if (!package_name.empty() && !octomap_path_file.empty()) { // try { -// const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); +// const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { -// return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); +// throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); // } // // if (octomap_ros::io::load_from_file(map_path_, octomap_)) { @@ -160,7 +153,7 @@ OctomapMapsManager::on_initialize() // } // incoming_occ_map_sub_ = node->create_subscription( -// node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", +// node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_occ_map", // rclcpp::QoS(1).transient_local().reliable(), // [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { // @@ -173,15 +166,19 @@ OctomapMapsManager::on_initialize() // octomap_pub_->publish(octomap_msg_); // }); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), - [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { + [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.05)); + tf_info.map_frame, msg->header.frame_id, msg->header.stamp, + rclcpp::Duration::from_seconds(0.05)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "OctomapMapsManager: TF failed: %s", ex.what()); return; @@ -196,9 +193,10 @@ OctomapMapsManager::on_initialize() pcl::fromROSMsg(*msg, *pcl_ds); pcl::VoxelGrid vg; - vg.setLeafSize(static_cast(resolution), - static_cast(resolution), - static_cast(resolution)); + vg.setLeafSize( + static_cast(resolution), + static_cast(resolution), + static_cast(resolution)); vg.filter(*pcl_ds); octomap_ = std::make_shared<::octomap::OcTree>(resolution); @@ -224,8 +222,7 @@ OctomapMapsManager::on_initialize() octomap_->insertPointCloud(cloud, origin, 1000.0, true, false); octomap_->updateInnerOccupancy(); - - octomap_msg_.header.frame_id = "map"; + octomap_msg_.header.frame_id = tf_info.map_frame; octomap_msg_.header.stamp = this->get_node()->now(); octomap_msg_.id = "OcTree"; octomap_msg_.binary = true; @@ -238,7 +235,7 @@ OctomapMapsManager::on_initialize() savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/savemap", [this]( const std::shared_ptr request, std::shared_ptr response) @@ -248,8 +245,6 @@ OctomapMapsManager::on_initialize() // ToDo }); - - return {}; } void diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp index c9cce305..949c5a83 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp @@ -39,12 +39,11 @@ *********************************************************************/ -#include #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" @@ -86,8 +85,9 @@ bool InflationFilter::inflate_layer_u8( if (!src_view || src_view->size() != nm.navcels.size()) { return false; } - auto dst_view = nm.layers.add_or_get(dst_layer, nm.navcels.size(), - layer_type_tag()); + auto dst_view = nm.layers.add_or_get( + dst_layer, nm.navcels.size(), + layer_type_tag()); if (!dst_view) {return false;} if (dst_view->data().size() != nm.navcels.size()) { const_cast &>(dst_view->data()).assign(nm.navcels.size(), FREE_SPACE); @@ -178,7 +178,7 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -194,11 +194,10 @@ InflationFilter::on_initialize() node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", inflation_radius_, cost_scaling_factor_); - - return {}; } void @@ -210,8 +209,9 @@ InflationFilter::update(::easynav::NavState & nav_state) octomap_ = nav_state.get<::octomap::Octomap>("map"); - if (!inflate_layer_u8(octomap_, "obstacles", "inflated_obstacles", - inflation_radius_, cost_scaling_factor_, 0.3)) + if (!inflate_layer_u8( + octomap_, "obstacles", "inflated_obstacles", + inflation_radius_, cost_scaling_factor_, 0.3)) { RCLCPP_ERROR(parent_node_->get_logger(), "Error inflating at ObstacleFilter"); } diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp index 8d6671fc..2561667f 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp @@ -1,29 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" @@ -40,11 +34,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(::easynav::NavState & nav_state) @@ -52,12 +44,15 @@ ObstacleFilter::update(::easynav::NavState & nav_state) if (!nav_state.has("map")) { return; } - if (!nav_state.has("points")) { + + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (!octomap_.layer_copy("occupancy", "obstacles")) { RCLCPP_ERROR(parent_node_->get_logger(), "Error copying layers at ObstacleFilter"); @@ -66,8 +61,8 @@ ObstacleFilter::update(::easynav::NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(get_map_resolution()) - .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .fuse(tf_info.map_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); size_t sidx = 0; diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp index 3c732e6d..1ea4a111 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp @@ -1,28 +1,21 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include #include #include "octomap/octomap.h" -#include "easynav_common/types/NavState.hpp" #include "easynav_octomap_maps_manager/filters/OctomapFilter.hpp" @@ -36,18 +29,14 @@ OctomapFilter::OctomapFilter() } -std::expected +void OctomapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; - - return on_initialize(); + on_initialize(); } std::shared_ptr @@ -62,11 +51,5 @@ OctomapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -OctomapFilter::get_tf_prefix() const -{ - return tf_prefix_; -} - } // namespace octomap } // namespace easynav diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp deleted file mode 100644 index 478f6f7e..00000000 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "easynav_octomap_maps_manager/utils.hpp" - -namespace easynav -{ -namespace octomap -{ - - -} // namespace octomap -} // namespace easynav diff --git a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst new file mode 100644 index 00000000..4c78f245 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst @@ -0,0 +1,37 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_simple_maps_manager +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Update calls to deprecated get_package_share_directory +* Navstate key among filters are always map, not an arbitrary key +* Merge pull request `#57 `_ from midemig/savemap_default + Added default path for route if not specified +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Final adjustements +* Refactor to use TFInfo +* Added missing dep +* Added missing licenses +* Tests and README +* Route Maps Manager with filters. Costmap filter +* Routes Maps manager finalized +* Add/Remove segment +* Interactive markers and ids in the segments +* Basic route server +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + +0.0.2 (2025-10-12) +------------------ +* Reorganization initial +* Contributors: Francisco Martín Rico diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt new file mode 100644 index 00000000..ca6c4211 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_routes_maps_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_costmap_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(std_srvs REQUIRED) +find_package(yaets REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(interactive_markers REQUIRED) + + +add_library(${PROJECT_NAME} SHARED + src/easynav_routes_maps_manager/RoutesMapsManager.cpp + src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + easynav_costmap_common::easynav_costmap_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + pluginlib::pluginlib + ament_index_cpp::ament_index_cpp + yaets::yaets + yaml-cpp + interactive_markers::interactive_markers + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_srvs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the plugins +pluginlib_export_plugin_description_file(easynav_core easynav_routes_maps_manager_plugins.xml) +pluginlib_export_plugin_description_file(easynav_routes_maps_manager easynav_routes_filters_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + rclcpp + rclcpp_lifecycle + tf2_ros + pluginlib + easynav_costmap_common + nav_msgs + ament_index_cpp + std_srvs + geometry_msgs + visualization_msgs + interactive_markers + yaets + yaml-cpp +) +ament_package() diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md new file mode 100644 index 00000000..3e1ba4f2 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -0,0 +1,155 @@ +# easynav_routes_maps_manager + +## Description + +Maps Manager that maintains a set of navigation routes (in 2D or 3D), exposes them as +visualization markers and interactive markers in RViz, and makes the current +`RoutesMap` available through the NavState so that other managers and filters +can consume it (for example, costmap filters that constrain navigation to a +given corridor). + +Routes are represented as a list of straight-line segments between two poses. +Nothing prevents you from using non-zero Z coordinates in the poses, so +routes can be fully 3D if downstream consumers support it. +They are typically loaded from a YAML file stored in a ROS package and can be +edited at runtime using interactive markers. A default single route segment is +created automatically when no routes file is configured. + +## Authors and Maintainers + +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Francisco Martín Rico + +## Supported ROS 2 Distributions + +| Distribution | Status | +|---|---| +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | +| kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) + +- **Plugin Name:** `easynav_routes_maps_manager/RoutesMapsManager` +- **Type:** `easynav::RoutesMapsManager` +- **Base Class:** `easynav::MapsManagerBase` +- **Library:** `easynav_routes_maps_manager` +- **Description:** Maintains an in-memory set of route segments (RoutesMap), + loads/saves them from/to YAML files, exposes them through ROS markers and + interactive markers, and writes the current `RoutesMap` into the NavState. + +In addition, this package provides the **RoutesCostmapFilter** plugin, which +can be used by the costmap Maps Manager to raise costs outside the defined +routes, effectively constraining navigation to a configurable corridor around +the segments. Other routes filters (for example, for NavMap or Octomap +representations) can be implemented in the same way, each using its own +NavState keys and outputs. + +## Parameters + +### Plugin Parameters (namespace: `//easynav_routes_maps_manager/RoutesMapsManager/...`) + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.package` | `string` | `""` | Package name used to resolve relative route YAML paths via `ament_index`. When empty and `.map_path_file` is also empty, a default in-memory route is created. | +| `.map_path_file` | `string` | `""` | Relative path (inside the package) or absolute path to a YAML file defining the routes (see *Routes YAML Format* below). | +| `.filters` | `string[]` | `[]` | List of routes filter identifiers to be instantiated (see *Routes Filter Plugins*). | +| `..plugin` | `string` | `""` | Type of routes filter plugin (e.g., `easynav_routes_maps_manager/RoutesCostmapFilter`). | + +If both `.package` and `.map_path_file` are empty, the manager +does not attempt to read a file and instead initializes a single default segment +from `(0, 0, 0)` to `(1, 0, 0)`. + +If only one of the parameters is set (package without path or path without +package for relative paths), initialization fails with an error. + +### Routes YAML Format + +Routes are loaded from a YAML file with the following structure: + +```yaml +routes: [route0, route1] + +route0: + start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + +route1: + start: {x: 1.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 2.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +``` + +- `routes`: ordered list of route segment identifiers. +- Each entry under `routeX` defines the start and end poses of a segment. + +When saving routes (via the save service, see below), the manager writes back a +file with the same structure. + +### Routes Filter Plugins + +Each entry in `.filters` defines a sub-namespace `.` +with at least the key `plugin`, plus any filter-specific parameters. Filters +implement the `RoutesFilter` interface and are executed after the `RoutesMap` +has been written to the NavState on each update cycle. + +#### RoutesCostmapFilter + +- **Plugin Name:** `easynav_routes_maps_manager/RoutesCostmapFilter` +- **Type:** `easynav::RoutesCostmapFilter` +- **Description:** + Reads the current `RoutesMap` and a dynamic costmap (`map`) + from the NavState and raises the cost of all cells that do not lie within a + corridor around any route segment. This effectively constrains path planners + to stay close to the defined routes. + +**Parameters (namespace: `//..././...`):** + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.min_cost` | `int` | `50` | Minimum cost assigned to cells outside the route corridor if their current cost is lower. | +| `.route_width` | `double` | `0.0` | Corridor half-width around the route segments in meters. When set to `0.0`, a default width equal to half the costmap cell diagonal is used. | + +**NavState usage in RoutesCostmapFilter:** + +| Key | Type | Access | Description | +|---|---|---|---| +| `routes` | `RoutesMap` | **Read** | In-memory set of route segments written by `RoutesMapsManager` on each update. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic costmap that is modified in place: cells outside the corridor are raised to at least `min_cost`. | + +In addition, the filter publishes a debug `nav_msgs/msg/OccupancyGrid` topic +with the filtered costmap for visualization. Other routes filters may use +different NavState keys or map representations; those should document their +own usage in their respective fichas. + +## Interfaces (Topics and Services) + +### Subscriptions and Publications + +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| +| Publisher | `//routes` | `visualization_msgs/msg/MarkerArray` | Publishes line and arrow markers representing all route segments for RViz visualization. | `depth=10, transient_local, reliable` | +| Publisher | `//routes_imarkers` | `visualization_msgs/msg/InteractiveMarker` | Publishes interactive markers for editing route endpoints (creation, removal, and dragging of segment endpoints). | `depth=1` | +| Publisher | `//routes_map` | `nav_msgs/msg/OccupancyGrid` | (via `RoutesCostmapFilter`) Debug occupancy grid showing the costmap after applying the route-based corridor filter. | `depth=1` | + +### Services + +| Direction | Service | Type | Purpose | +|---|---|---|---| +| Service Server | `//save_routes` | `std_srvs/srv/Trigger` | Serializes the current set of routes to the configured YAML file on disk. | + +## NavState Keys + +| Key | Type | Access | Notes | +|---|---|---|---| +| `routes` | `RoutesMap` | **Write** | In-memory set of route segments loaded from YAML or created interactively. Written by `RoutesMapsManager` on each update. | + +## TF Frames + +Routes are expressed in the navigation `map` frame (or in the frame configured +in other managers that consume the `RoutesMap`). This plugin does not broadcast +TF transforms; it relies on the rest of the navigation stack to provide the +necessary TF tree. + +## License + +Apache-2.0 diff --git a/maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml b/maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml new file mode 100644 index 00000000..a000cdd4 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml @@ -0,0 +1,9 @@ + + + + + Routes-based costmap filter that enforces a minimum cost outside route segments. + + + + diff --git a/maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml b/maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml new file mode 100644 index 00000000..f286078c --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml @@ -0,0 +1,11 @@ + + + + + A default "routes" implementation for the Maps Manager. + This maps manager does nothing. It serves as an example, and will be used as a default plugin implementation + if the navigation system configuration does not specify one. + + + + diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp new file mode 100644 index 00000000..d194e338 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp @@ -0,0 +1,74 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ + +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace easynav +{ + +class NavState; + +/// @brief Base interface for all routes filters. +/// +/// A RoutesFilter consumes the current navigation state (including the +/// active RoutesMap written by RoutesMapsManager) and is allowed to +/// modify other entries in the NavState (for example, a costmap) in +/// order to enforce corridor-following behaviour, masking, etc. +class RoutesFilter +{ +public: + /// @brief Shared pointer alias for convenience. + using Ptr = std::shared_ptr; + + /// @brief Virtual destructor. + virtual ~RoutesFilter() = default; + + /// @brief Configure the filter instance. + /// + /// This method is called once after construction by the + /// RoutesMapsManager. Implementations should declare/read any + /// required parameters, create publishers or other resources, and + /// store references to the lifecycle node. + /// + /// @param node Shared pointer to the owning lifecycle node. + /// @param plugin_ns Namespace under which this filter is configured + /// (used as prefix for ROS parameters and topics). + /// @param tf_info TF frame information used by the navigation stack. + /// @throws std::runtime_error if initialization fails. + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns) = 0; + + /// @brief Update hook called every navigation cycle. + /// + /// Implementations may read the current "routes" entry from the + /// NavState as well as other map representations and modify them in + /// place. The filter must not assume ownership of data stored in the + /// NavState. + /// + /// @param nav_state Blackboard-like navigation state container. + virtual void update(NavState & nav_state) = 0; +}; + +} // namespace easynav + +#endif // EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp new file mode 100644 index 00000000..e7d56c86 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp @@ -0,0 +1,154 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Declaration of the RoutesMapsManager class and related types. + +#ifndef EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ +#define EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ + +#include "geometry_msgs/msg/pose.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" + +#include "interactive_markers/interactive_marker_server.hpp" + +#include "std_srvs/srv/trigger.hpp" + +#include +#include + +#include "pluginlib/class_loader.hpp" + +#include "easynav_core/MapsManagerBase.hpp" + +#include "easynav_routes_maps_manager/RoutesFilter.hpp" + +namespace easynav +{ + +/// @brief Simple directed segment between two poses. +/// +/// Each RouteSegment represents a straight-line connection between two +/// poses in the navigation frame. The segment can be individually +/// edited and identified via its @ref id field. +struct RouteSegment +{ + /// @brief Unique identifier for this segment. + std::string id; + + /// @brief Start pose of the segment. + geometry_msgs::msg::Pose start; + + /// @brief End pose of the segment. + geometry_msgs::msg::Pose end; + + /// @brief Whether this segment is currently in edit mode. + bool edit_mode{false}; +}; + +/// @brief Container type representing a full set of navigation routes. +using RoutesMap = std::vector; + +/** + * @class RoutesMapsManager + * @brief A plugin-based map manager using the RoutesMap data structure. + * + * This manager implements a minimal mapping approach using a set of predefined + * navigation routes (RoutesMap). Routes are defined as straight-line segments + * between two poses and are loaded from a YAML file. The segments are + * visualized using MarkerArray messages. + */ +class RoutesMapsManager : public easynav::MapsManagerBase +{ +public: + /** + * @brief Default constructor. + */ + RoutesMapsManager(); + + /** + * @brief Destructor. + */ + ~RoutesMapsManager(); + + /** + * @brief Initializes the maps manager. + * + * Creates necessary publishers/subscribers and initializes the map instances. + * + * @throws std::runtime_error if initialization fails. + */ + virtual void on_initialize() override; + + /** + * @brief Updates the internal maps using the current navigation state. + * + * Intended to be called periodically. May perform dynamic map updates + * based on new sensor data or internal state. + * + * @param nav_state Current state of the navigation system. + */ + virtual void update(NavState & nav_state) override; + + /// @brief Access to the loaded routes (for testing and tools). + const RoutesMap & get_routes() const {return routes_;} + +private: + /// @brief Load routes from the configured YAML file into @ref routes_. + /// + /// When the configured YAML file is missing, invalid, or the + /// "routes" key is not present, a default map with a single segment + /// from (0, 0, 0) to (1, 0, 0) is created instead. + void load_routes_from_yaml(); + + /// @brief Publish the current routes as visualization markers. + void publish_routes_markers(); + + /// @brief Publish or update interactive markers for editing endpoints. + void publish_interactive_markers(); + + /// @brief Handle feedback from interactive markers and update @ref routes_. + void handle_interactive_feedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); + /// @brief Full path to the YAML file used to load/save routes. + std::string map_path_; + + /// @brief In-memory collection of route segments. + RoutesMap routes_; + + /// @brief Monotonic counter for generating unique route IDs. + int next_route_id_{0}; + + /// @brief Plugin loader for route filters. + std::unique_ptr> routes_filters_loader_; + + /// @brief Loaded route filters. + std::vector> routes_filters_; + + /// @brief Publisher for visualizing routes as markers. + rclcpp::Publisher::SharedPtr routes_pub_; + + /// @brief Interactive marker server for editing endpoints. + std::shared_ptr imarker_server_; + + /// @brief Service for saving current routes back to disk. + rclcpp::Service::SharedPtr save_routes_srv_; +}; + +} // namespace easynav + +#endif // EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp new file mode 100644 index 00000000..975883ba --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp @@ -0,0 +1,94 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ + +#include + +#include "easynav_routes_maps_manager/RoutesFilter.hpp" + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace easynav +{ + +/// @brief Costmap filter that raises costs outside navigation routes. +/// +/// This filter reads the current RoutesMap and a dynamic costmap +/// ("map") from the NavState. All cells that do not +/// lie within a configurable corridor around any route segment have +/// their cost raised to at least @c min_cost_. A debug occupancy grid +/// representing the filtered costmap is also published. +class RoutesCostmapFilter : public RoutesFilter +{ +public: + /// @brief Default constructor. + RoutesCostmapFilter(); + + /// @brief Configure the costmap filter. + /// + /// Declares and reads plugin-specific parameters (such as + /// @c min_cost and @c route_width), and creates the debug + /// OccupancyGrid publisher. + /// + /// @param node Owning lifecycle node provided by the + /// RoutesMapsManager. + /// @param plugin_ns Namespace used to resolve this filter's + /// parameters and topics. + /// @param tf_info TF frame information used by the navigation stack. + /// @throws std::runtime_error if initialization fails. + void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns) override; + + /// @brief Apply the routes-based filtering to the costmap. + /// + /// If both "routes" and "map" entries are present + /// in the NavState, the costmap is modified in place and the debug + /// occupancy grid is published. If either entry is missing, the + /// function returns without making changes. + /// + /// @param nav_state Current navigation state containing the routes + /// and costmap to be filtered. + void update(NavState & nav_state) override; + +private: + /// @brief Weak reference to the owning lifecycle node. + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + /// @brief Plugin namespace used for parameters and topics. + std::string plugin_ns_; + + /// @brief Minimum cost enforced outside the route corridor. + int min_cost_{50}; + + /// @brief Corridor half-width around route segments in meters. + /// + /// When set to @c 0.0, a default corresponding to half of the + /// costmap cell diagonal is used. + double route_width_{0.0}; + + /// @brief Publisher for the debug occupancy grid representing the + /// filtered costmap. + rclcpp::Publisher::SharedPtr routes_occ_pub_; + + /// @brief Reusable message used to publish the debug grid. + nav_msgs::msg::OccupancyGrid routes_grid_msg_; +}; + +} // namespace easynav + +#endif // EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml new file mode 100644 index 00000000..be6c1b61 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -0,0 +1,35 @@ + + + + easynav_routes_maps_manager + 0.4.0 + Easy Navigation: Routes MapsManager package. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_lifecycle + easynav_common + easynav_core + easynav_costmap_common + tf2_ros + pluginlib + nav_msgs + ament_index_cpp + std_srvs + geometry_msgs + visualization_msgs + interactive_markers + yaets + yaml-cpp + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp new file mode 100644 index 00000000..10eac533 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -0,0 +1,807 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include + +#include + +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace easynav +{ + +RoutesMapsManager::RoutesMapsManager() +{ + NavState::register_printer( + [](const RoutesMap & routes) { + std::ostringstream out; + out << "RoutesMap with " << routes.size() << " segments"; + for (std::size_t i = 0; i < routes.size(); ++i) { + const auto & s = routes[i]; + out << "\n [" << i << "] from (" << s.start.position.x << ", " + << s.start.position.y << ") to (" << s.end.position.x << ", " + << s.end.position.y << ")"; + } + return out.str(); + }); + + routes_filters_loader_ = std::make_unique>( + "easynav_routes_maps_manager", "easynav::RoutesFilter"); +} + +RoutesMapsManager::~RoutesMapsManager() = default; + +void RoutesMapsManager::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + std::string package_name, map_path_file; + if (!node->has_parameter(plugin_name + ".package")) { + node->declare_parameter(plugin_name + ".package", package_name); + } + if (!node->has_parameter(plugin_name + ".map_path_file")) { + node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + } + + node->get_parameter(plugin_name + ".package", package_name); + node->get_parameter(plugin_name + ".map_path_file", map_path_file); + + // Load route filters plugins configuration + std::vector routes_filters_names; + if (!node->has_parameter(plugin_name + ".filters")) { + node->declare_parameter(plugin_name + ".filters", routes_filters_names); + } + node->get_parameter(plugin_name + ".filters", routes_filters_names); + + map_path_.clear(); + if (package_name.empty() && map_path_file.empty()) { + // No path configured: default to /tmp so that save_routes still works. + map_path_ = "/tmp/routes.yaml"; + RCLCPP_INFO( + node->get_logger(), + "[%s] No package or map_path_file specified, routes will be saved to %s", + plugin_name.c_str(), map_path_.c_str()); + } else if (!map_path_file.empty() && map_path_file[0] == '/') { + // Absolute path: ignore package_name. + map_path_ = map_path_file; + } else if (!package_name.empty() && !map_path_file.empty()) { + const std::filesystem::path pkgpath(ament_index_cpp::get_package_share_directory(package_name)); + map_path_ = (pkgpath / map_path_file).string(); + } else { + throw std::runtime_error( + "Parameters '" + plugin_name + ".package' and '" + plugin_name + + ".map_path_file' are not correctly set"); + } + + routes_pub_ = node->create_publisher( + node->get_name() + std::string("/") + plugin_name + "/routes", + rclcpp::QoS(10).transient_local().reliable()); + + imarker_server_ = std::make_shared( + plugin_name + std::string("_imarkers"), node, false); + + save_routes_srv_ = node->create_service( + node->get_name() + std::string("/") + plugin_name + "/save_routes", + [this](const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) { + try { + // Persist current routes_ back to YAML file using the + // structure: + // routes: [route1, route2] + // route1: { start: ..., end: ... } + YAML::Emitter out; + out << YAML::BeginMap; + + // Collect route names from ids (or generate generic ones). + std::vector names; + names.reserve(routes_.size()); + for (std::size_t i = 0; i < routes_.size(); ++i) { + const auto & seg = routes_[i]; + if (!seg.id.empty()) { + names.push_back(seg.id); + } else { + names.push_back("route" + std::to_string(i)); + } + } + + out << YAML::Key << "routes" << YAML::Value << YAML::Flow << YAML::BeginSeq; + for (const auto & n : names) { + out << n; + } + out << YAML::EndSeq; + + // Now define each route as a separate key in the map. + for (std::size_t i = 0; i < routes_.size(); ++i) { + const auto & seg = routes_[i]; + const auto & name = names[i]; + + out << YAML::Key << name << YAML::Value << YAML::BeginMap; + + out << YAML::Key << "start" << YAML::Value << YAML::BeginMap; + out << YAML::Key << "x" << YAML::Value << seg.start.position.x; + out << YAML::Key << "y" << YAML::Value << seg.start.position.y; + out << YAML::Key << "z" << YAML::Value << seg.start.position.z; + out << YAML::Key << "qx" << YAML::Value << seg.start.orientation.x; + out << YAML::Key << "qy" << YAML::Value << seg.start.orientation.y; + out << YAML::Key << "qz" << YAML::Value << seg.start.orientation.z; + out << YAML::Key << "qw" << YAML::Value << seg.start.orientation.w; + out << YAML::EndMap; + + out << YAML::Key << "end" << YAML::Value << YAML::BeginMap; + out << YAML::Key << "x" << YAML::Value << seg.end.position.x; + out << YAML::Key << "y" << YAML::Value << seg.end.position.y; + out << YAML::Key << "z" << YAML::Value << seg.end.position.z; + out << YAML::Key << "qx" << YAML::Value << seg.end.orientation.x; + out << YAML::Key << "qy" << YAML::Value << seg.end.orientation.y; + out << YAML::Key << "qz" << YAML::Value << seg.end.orientation.z; + out << YAML::Key << "qw" << YAML::Value << seg.end.orientation.w; + out << YAML::EndMap; + + out << YAML::EndMap; + } + + out << YAML::EndMap; + + std::ofstream file(map_path_); + if (!file.is_open()) { + response->success = false; + response->message = "Could not open file for writing: " + map_path_; + return; + } + file << out.c_str(); + file.close(); + + response->success = true; + response->message = "Routes saved to " + map_path_; + } catch (const std::exception & e) { + response->success = false; + response->message = e.what(); + } + }); + + try { + load_routes_from_yaml(); + publish_routes_markers(); + publish_interactive_markers(); + } catch (const std::exception & e) { + throw std::runtime_error(std::string{"Failed to load routes: "} + e.what()); + } + + // Instantiate and initialize configured route filters + for (const auto & filter_name : routes_filters_names) { + std::string plugin; + if (!node->has_parameter(plugin_name + "." + filter_name + ".plugin")) { + node->declare_parameter(plugin_name + "." + filter_name + ".plugin", plugin); + } + node->get_parameter(plugin_name + "." + filter_name + ".plugin", plugin); + + if (plugin.empty()) { + RCLCPP_WARN( + node->get_logger(), + "RoutesMapsManager: plugin parameter for filter '%s' is empty", filter_name.c_str()); + continue; + } + + try { + RCLCPP_INFO( + node->get_logger(), + "Loading RoutesFilter %s [%s]", filter_name.c_str(), plugin.c_str()); + + std::shared_ptr instance = + routes_filters_loader_->createSharedInstance(plugin); + + try { + instance->initialize(node, plugin_name + "." + filter_name); + } catch (const std::exception & e) { + RCLCPP_ERROR( + node->get_logger(), + "Unable to initialize RoutesFilter %s [%s]. Error: %s", + filter_name.c_str(), plugin.c_str(), e.what()); + throw std::runtime_error( + "Unable to initialize RoutesFilter " + plugin + + " . Error: " + e.what()); + } + + routes_filters_.push_back(instance); + + RCLCPP_INFO( + node->get_logger(), + "Loaded RoutesFilter %s [%s]", filter_name.c_str(), plugin.c_str()); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Unable to load plugin easynav::RoutesFilter. Error: %s", ex.what()); + throw std::runtime_error( + "Unable to load plugin easynav::RoutesFilter " + + filter_name + " . Error: " + ex.what()); + } + } +} + +void RoutesMapsManager::update(NavState & nav_state) +{ + // Expose routes map through NavState for other modules. + nav_state.set("routes", routes_); + + // Let route filters apply their effects based on current routes and nav state. + for (const auto & filter : routes_filters_) { + filter->update(nav_state); + } +} + +void RoutesMapsManager::load_routes_from_yaml() +{ + routes_.clear(); + + if (map_path_.empty()) { + // No map path configured: initialize a default single route segment. + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + + routes_.push_back(segment); + next_route_id_ = 1; + return; + } + + YAML::Node root; + try { + root = YAML::LoadFile(map_path_); + } catch (const std::exception &) { + // File missing or invalid: fall back to a default single route. + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + + routes_.push_back(segment); + next_route_id_ = 1; + return; + } + + if (!root["routes"]) { + // No explicit routes list: use a default single route. + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + + routes_.push_back(segment); + next_route_id_ = 1; + return; + } + + // routes: [route1, route2, ...] + const auto & names_node = root["routes"]; + for (std::size_t i = 0; i < names_node.size(); ++i) { + const auto name = names_node[i].as(); + + if (!root[name]) { + continue; + } + + const auto & route_node = root[name]; + if (!route_node["start"] || !route_node["end"]) { + continue; + } + + RouteSegment segment; + segment.id = name; + + const auto & start = route_node["start"]; + const auto & end = route_node["end"]; + + segment.start.position.x = start["x"].as(); + segment.start.position.y = start["y"].as(); + segment.start.position.z = start["z"].as(0.0); + + segment.start.orientation.x = start["qx"].as(0.0); + segment.start.orientation.y = start["qy"].as(0.0); + segment.start.orientation.z = start["qz"].as(0.0); + segment.start.orientation.w = start["qw"].as(1.0); + + segment.end.position.x = end["x"].as(); + segment.end.position.y = end["y"].as(); + segment.end.position.z = end["z"].as(0.0); + + segment.end.orientation.x = end["qx"].as(0.0); + segment.end.orientation.y = end["qy"].as(0.0); + segment.end.orientation.z = end["qz"].as(0.0); + segment.end.orientation.w = end["qw"].as(1.0); + + routes_.push_back(segment); + } + + // Initialize next_route_id_ so that newly created routes get + // unique IDs that don't clash with existing ones. + next_route_id_ = 0; + for (const auto & seg : routes_) { + if (seg.id.rfind("route", 0) == 0 && seg.id.size() > 5) { + try { + const int n = std::stoi(seg.id.substr(5)); + if (n >= next_route_id_) { + next_route_id_ = n + 1; + } + } catch (...) { + // Non-numeric suffixes are ignored. + } + } + } +} + +void RoutesMapsManager::publish_routes_markers() +{ + if (!routes_pub_) { + return; + } + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + visualization_msgs::msg::MarkerArray array; + + // First, delete all previous markers in our namespaces so that + // removed segments do not leave orphaned markers behind. + { + visualization_msgs::msg::Marker m; + m.header.frame_id = tf_info.map_frame; + m.action = visualization_msgs::msg::Marker::DELETEALL; + + m.ns = "routes_line"; + array.markers.push_back(m); + + m.ns = "routes_arrow"; + array.markers.push_back(m); + } + + int id = 0; + for (const auto & seg : routes_) { + // Line between start and end + visualization_msgs::msg::Marker line; + line.header.frame_id = tf_info.map_frame; + line.ns = "routes_line"; + line.id = id++; + line.type = visualization_msgs::msg::Marker::LINE_LIST; + line.action = visualization_msgs::msg::Marker::ADD; + + line.scale.x = 0.05; // line width + + line.color.r = 0.0f; + line.color.g = 1.0f; + line.color.b = 0.0f; + line.color.a = 1.0f; + + line.points.resize(2); + line.points[0].x = seg.start.position.x; + line.points[0].y = seg.start.position.y; + line.points[0].z = seg.start.position.z; + + line.points[1].x = seg.end.position.x; + line.points[1].y = seg.end.position.y; + line.points[1].z = seg.end.position.z; + + array.markers.push_back(line); + + // Arrow for start orientation (same style as end) + visualization_msgs::msg::Marker start_arrow; + start_arrow.header.frame_id = tf_info.map_frame; + start_arrow.ns = "routes_arrow"; + start_arrow.id = id++; + start_arrow.type = visualization_msgs::msg::Marker::ARROW; + start_arrow.action = visualization_msgs::msg::Marker::ADD; + start_arrow.pose = seg.start; + start_arrow.scale.x = 0.25; // shaft length + start_arrow.scale.y = 0.05; // shaft diameter + start_arrow.scale.z = 0.1; // head diameter + start_arrow.color.r = 1.0f; + start_arrow.color.g = 1.0f; + start_arrow.color.b = 0.0f; + start_arrow.color.a = 0.9f; + array.markers.push_back(start_arrow); + + // Arrow for end orientation (same style) + visualization_msgs::msg::Marker end_arrow; + end_arrow.header.frame_id = tf_info.map_frame; + end_arrow.ns = "routes_arrow"; + end_arrow.id = id++; + end_arrow.type = visualization_msgs::msg::Marker::ARROW; + end_arrow.action = visualization_msgs::msg::Marker::ADD; + end_arrow.pose = seg.end; + end_arrow.scale.x = 0.25; + end_arrow.scale.y = 0.05; + end_arrow.scale.z = 0.1; + end_arrow.color.r = 1.0f; + end_arrow.color.g = 1.0f; + end_arrow.color.b = 0.0f; + end_arrow.color.a = 0.9f; + array.markers.push_back(end_arrow); + } + + routes_pub_->publish(array); +} + +void RoutesMapsManager::publish_interactive_markers() +{ + if (!imarker_server_) { + return; + } + imarker_server_->clear(); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + for (const auto & seg : routes_) { + // Per-segment toggle cube (red in normal mode, green in edit mode). + visualization_msgs::msg::InteractiveMarker mode_marker; + mode_marker.header.frame_id = tf_info.map_frame; + mode_marker.name = seg.id + "_mode"; + mode_marker.scale = 1.0; + + // Mid-point between start and end (no vertical offset) + mode_marker.pose.position.x = 0.5 * (seg.start.position.x + seg.end.position.x); + mode_marker.pose.position.y = 0.5 * (seg.start.position.y + seg.end.position.y); + mode_marker.pose.position.z = 0.5 * (seg.start.position.z + seg.end.position.z); + + visualization_msgs::msg::InteractiveMarkerControl mode_ctrl; + mode_ctrl.name = "toggle_edit"; + mode_ctrl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::BUTTON; + mode_ctrl.always_visible = true; + + visualization_msgs::msg::Marker cube; + cube.type = visualization_msgs::msg::Marker::CUBE; + cube.scale.x = 0.15; // even smaller cube + cube.scale.y = 0.15; + cube.scale.z = 0.15; + if (!seg.edit_mode) { + // Red in normal mode + cube.color.r = 1.0f; + cube.color.g = 0.0f; + cube.color.b = 0.0f; + } else { + // Green in edit mode + cube.color.r = 0.0f; + cube.color.g = 1.0f; + cube.color.b = 0.0f; + } + cube.color.a = 0.9f; + + // Text label for the toggle control + visualization_msgs::msg::Marker toggle_text; + toggle_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + toggle_text.text = "toggle edit"; + toggle_text.scale.z = 0.1; // font height + toggle_text.color.r = 1.0f; + toggle_text.color.g = 1.0f; + toggle_text.color.b = 1.0f; + toggle_text.color.a = 1.0f; + toggle_text.pose.position.z = 0.3; // slightly above the cube + + mode_ctrl.markers.push_back(cube); + mode_ctrl.markers.push_back(toggle_text); + + mode_marker.controls.push_back(mode_ctrl); + + imarker_server_->insert( + mode_marker, + std::bind( + &RoutesMapsManager::handle_interactive_feedback, + this, + std::placeholders::_1)); + + if (!seg.edit_mode) { + // In normal mode we only show the cube and skip endpoint controls. + continue; + } + + visualization_msgs::msg::InteractiveMarker start_marker; + start_marker.header.frame_id = tf_info.map_frame; + start_marker.name = seg.id + "_start"; + start_marker.description = "Route " + seg.id + " start"; + start_marker.pose = seg.start; + start_marker.scale = 1.0; + + visualization_msgs::msg::InteractiveMarker end_marker; + end_marker.header.frame_id = tf_info.map_frame; + end_marker.name = seg.id + "_end"; + end_marker.description = "Route " + seg.id + " end"; + end_marker.pose = seg.end; + end_marker.scale = 1.0; + + auto add_controls = [](visualization_msgs::msg::InteractiveMarker & marker) { + visualization_msgs::msg::InteractiveMarkerControl control; + + // Move along X + control.orientation.w = 1.0; + control.orientation.x = 1.0; + control.orientation.y = 0.0; + control.orientation.z = 0.0; + control.name = "move_x"; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + marker.controls.push_back(control); + + // Move along Y + control.orientation.x = 0.0; + control.orientation.y = 1.0; + control.name = "move_y"; + marker.controls.push_back(control); + + // Move along Z + control.orientation.y = 0.0; + control.orientation.z = 1.0; + control.name = "move_z"; + marker.controls.push_back(control); + + // Rotate around Z (yaw), orientation as in interactive_markers examples + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + control.orientation.w = 1.0; + control.orientation.x = 0.0; + control.orientation.y = 1.0; + control.orientation.z = 0.0; + control.name = "rotate_z"; + marker.controls.push_back(control); + + // Button control to add a new segment starting from this endpoint + visualization_msgs::msg::InteractiveMarkerControl add_ctrl; + add_ctrl.name = "add_segment"; + add_ctrl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::BUTTON; + add_ctrl.always_visible = true; + + visualization_msgs::msg::Marker add_marker; + add_marker.type = visualization_msgs::msg::Marker::SPHERE; + // Visual sphere for the add control + add_marker.scale.x = 0.2; + add_marker.scale.y = 0.2; + add_marker.scale.z = 0.2; + add_marker.color.r = 1.0f; + add_marker.color.g = 0.5f; + add_marker.color.b = 0.0f; + add_marker.color.a = 0.9f; + + // Text label for the add control + visualization_msgs::msg::Marker add_text; + add_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + add_text.text = "add"; + add_text.scale.z = 0.1; // font height (half) + add_text.color.r = 1.0f; + add_text.color.g = 1.0f; + add_text.color.b = 1.0f; + add_text.color.a = 1.0f; + add_text.pose.position.z = 0.4; // slightly above the sphere + + add_ctrl.markers.push_back(add_marker); + add_ctrl.markers.push_back(add_text); + + marker.controls.push_back(add_ctrl); + + // Button control to remove the segment this endpoint belongs to + visualization_msgs::msg::InteractiveMarkerControl remove_ctrl; + remove_ctrl.name = "remove_segment"; + remove_ctrl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::BUTTON; + remove_ctrl.always_visible = true; + + visualization_msgs::msg::Marker remove_marker; + remove_marker.type = visualization_msgs::msg::Marker::SPHERE; + // Place the red sphere 1 m above the endpoint so that it + // does not overlap with the orange "add" sphere. + remove_marker.pose.position.z = 1.0; + remove_marker.scale.x = 0.15; + remove_marker.scale.y = 0.15; + remove_marker.scale.z = 0.15; + remove_marker.color.r = 1.0f; + remove_marker.color.g = 0.0f; + remove_marker.color.b = 0.0f; + remove_marker.color.a = 0.9f; + + // Text label for the remove control + visualization_msgs::msg::Marker remove_text; + remove_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + remove_text.text = "remove"; + remove_text.scale.z = 0.1; // font height (half) + remove_text.color.r = 1.0f; + remove_text.color.g = 1.0f; + remove_text.color.b = 1.0f; + remove_text.color.a = 1.0f; + remove_text.pose.position.z = 1.3; // slightly above the red sphere + + remove_ctrl.markers.push_back(remove_marker); + remove_ctrl.markers.push_back(remove_text); + + marker.controls.push_back(remove_ctrl); + }; + + add_controls(start_marker); + add_controls(end_marker); + + imarker_server_->insert( + start_marker, + std::bind( + &RoutesMapsManager::handle_interactive_feedback, + this, + std::placeholders::_1)); + imarker_server_->insert( + end_marker, + std::bind( + &RoutesMapsManager::handle_interactive_feedback, + this, + std::placeholders::_1)); + } + + imarker_server_->applyChanges(); +} + +void RoutesMapsManager::handle_interactive_feedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ + if (!feedback) { + return; + } + + const auto & name = feedback->marker_name; + + // Toggle per-segment edit mode when clicking the central cube. + if (feedback->control_name == "toggle_edit" && + (feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK || + feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP)) + { + // name is _mode + const auto underscore_pos = name.rfind("_"); + if (underscore_pos != std::string::npos) { + const auto base_id = name.substr(0, underscore_pos); + for (auto & seg : routes_) { + if (seg.id == base_id) { + seg.edit_mode = !seg.edit_mode; + break; + } + } + publish_interactive_markers(); + } + return; + } + + // Creation of a new segment from this endpoint + if (feedback->control_name == "add_segment" && + (feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK || + feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP)) + { + // Compute forward direction from marker orientation (assume x-forward). + const auto & p = feedback->pose.position; + const auto & q = feedback->pose.orientation; + + const double qx = q.x; + const double qy = q.y; + const double qz = q.z; + const double qw = q.w; + + // Forward vector in world coordinates: q * (1,0,0) * q^{-1} + const double fx = 2.0 * (qx * qx + qw * qw) - 1.0; + const double fy = 2.0 * (qx * qy + qw * qz); + const double fz = 2.0 * (qx * qz - qw * qy); + + const double length = 2.0; // meters + + RouteSegment new_seg; + // New unique segment id based on a monotonic counter + new_seg.id = "route" + std::to_string(next_route_id_++); + + new_seg.start = feedback->pose; + new_seg.end = feedback->pose; + new_seg.end.position.x = p.x + fx * length; + new_seg.end.position.y = p.y + fy * length; + new_seg.end.position.z = p.z + fz * length; + + routes_.push_back(new_seg); + + publish_routes_markers(); + publish_interactive_markers(); + return; + } + + // Removal of the segment this endpoint belongs to + if (feedback->control_name == "remove_segment" && + (feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK || + feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP)) + { + // marker_name is either _start or _end + const auto underscore_pos = name.rfind("_"); + if (underscore_pos != std::string::npos) { + const auto base_id = name.substr(0, underscore_pos); + for (auto it = routes_.begin(); it != routes_.end(); ++it) { + if (it->id == base_id) { + routes_.erase(it); + break; + } + } + publish_routes_markers(); + publish_interactive_markers(); + } + return; + } + + for (auto & seg : routes_) { + if (name == seg.id + "_start") { + seg.start = feedback->pose; + publish_routes_markers(); + publish_interactive_markers(); + return; + } else if (name == seg.id + "_end") { + seg.end = feedback->pose; + publish_routes_markers(); + publish_interactive_markers(); + return; + } + } +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::RoutesMapsManager, easynav::MapsManagerBase) diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp new file mode 100644 index 00000000..b512cb3c --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp @@ -0,0 +1,188 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp" + +#include +#include +#include + +#include "easynav_common/types/NavState.hpp" + +#include "easynav_costmap_common/costmap_2d.hpp" + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +namespace easynav +{ + +RoutesCostmapFilter::RoutesCostmapFilter() = default; + +void +RoutesCostmapFilter::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns) +{ + node_ = node; + plugin_ns_ = plugin_ns; + // Parameter for minimum cost to apply outside routes + if (!node->has_parameter(plugin_ns_ + ".min_cost")) { + node->declare_parameter(plugin_ns_ + ".min_cost", min_cost_); + } + node->get_parameter(plugin_ns_ + ".min_cost", min_cost_); + + // Width of the route corridor around the segment (meters). + // If set to 0.0 (default), a width of one cell is used. + if (!node->has_parameter(plugin_ns_ + ".route_width")) { + node->declare_parameter(plugin_ns_ + ".route_width", route_width_); + } + node->get_parameter(plugin_ns_ + ".route_width", route_width_); + + // Publisher for the routes-influenced occupancy grid (for debugging/visualization) + // Topic: ///routes_map + // plugin_ns_ is typically ".", so we extract the + // base plugin_name (e.g. "routes" from "routes.routes_costmap"). + const auto dot_pos = plugin_ns_.find('.'); + const std::string plugin_name = + (dot_pos == std::string::npos) ? plugin_ns_ : plugin_ns_.substr(0, dot_pos); + + routes_occ_pub_ = node->create_publisher( + node->get_name() + std::string("/") + plugin_name + "/routes_map", + rclcpp::QoS(1).reliable()); +} + +void +RoutesCostmapFilter::update(NavState & nav_state) +{ + if (!nav_state.has("routes")) { + return; + } + + if (!nav_state.has("map")) { + return; + } + + const auto & routes = nav_state.get("routes"); + + auto costmap_ptr = nav_state.get_ptr("map"); + if (!costmap_ptr) { + return; + } + + Costmap2D & costmap = *costmap_ptr; + + const double resolution = costmap.getResolution(); + const double origin_x = costmap.getOriginX(); + const double origin_y = costmap.getOriginY(); + + const unsigned int size_x = costmap.getSizeInCellsX(); + const unsigned int size_y = costmap.getSizeInCellsY(); + + // Precompute a list of segments in world coordinates + struct Segment2D + { + double x0, y0, x1, y1; + }; + + std::vector segments; + segments.reserve(routes.size()); + for (const auto & seg : routes) { + segments.push_back( + { + seg.start.position.x, + seg.start.position.y, + seg.end.position.x, + seg.end.position.y}); + } + + auto cellHasRoute = [&segments, resolution, origin_x, origin_y, this]( + unsigned int mx, unsigned int my) -> bool + { + // Cell center in world coordinates + const double cx = origin_x + (mx + 0.5) * resolution; + const double cy = origin_y + (my + 0.5) * resolution; + + for (const auto & s : segments) { + // Distance from (cx,cy) to segment (s.x0,s.y0)-(s.x1,s.y1) + const double vx = s.x1 - s.x0; + const double vy = s.y1 - s.y0; + const double wx = cx - s.x0; + const double wy = cy - s.y0; + + const double c1 = vx * wx + vy * wy; + const double c2 = vx * vx + vy * vy; + + // Allow projection also before the start point (t >= 0) + if (c2 <= 0.0) { + continue; + } + const double t = c1 / c2; + if (t < 0.0 || t > 1.0) { + continue; + } + + const double proj_x = s.x0 + t * vx; + const double proj_y = s.y0 + t * vy; + + const double dx = cx - proj_x; + const double dy = cy - proj_y; + const double dist2 = dx * dx + dy * dy; + + // Consider the cell as on the route if the distance is less than + // the configured route width (or, if zero, half the cell diagonal). + double max_dist = route_width_; + if (max_dist <= 0.0) { + max_dist = std::sqrt(2.0) * resolution * 0.5; + } + + // Make the boundary slightly inclusive with a small epsilon to + // avoid dropping edge cells due to numerical issues. + constexpr double epsilon = 1e-6; + if (dist2 <= (max_dist * max_dist) + epsilon) { + return true; + } + } + + return false; + }; + + for (unsigned int y = 0; y < size_y; ++y) { + for (unsigned int x = 0; x < size_x; ++x) { + if (cellHasRoute(x, y)) { + // Do not modify cost on route + continue; + } + + unsigned char old_cost = costmap.getCost(x, y); + if (old_cost < static_cast(min_cost_)) { + costmap.setCost(x, y, static_cast(min_cost_)); + } + } + } + + costmap.toOccupancyGridMsg(routes_grid_msg_); + routes_grid_msg_.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; + if (auto node_locked = node_.lock()) { + routes_grid_msg_.header.stamp = node_locked->now(); + } + routes_occ_pub_->publish(routes_grid_msg_); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::RoutesCostmapFilter, easynav::RoutesFilter) diff --git a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt new file mode 100644 index 00000000..3638b4d4 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -0,0 +1,19 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(routes_mapsmanager_tests + routes_mapsmanager_tests.cpp) +target_link_libraries(routes_mapsmanager_tests + ${PROJECT_NAME} + easynav_common::easynav_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle) + +ament_add_gtest(routes_costmap_filter_tests + routes_costmap_filter_tests.cpp) +target_link_libraries(routes_costmap_filter_tests + ${PROJECT_NAME} + easynav_common::easynav_common + easynav_costmap_common::easynav_costmap_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle) + diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp new file mode 100644 index 00000000..011e462e --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp @@ -0,0 +1,161 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_costmap_common/costmap_2d.hpp" + +#include "easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp" + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" // for RoutesMap and RouteSegment typedefs + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using easynav::RoutesCostmapFilter; +using easynav::RoutesMap; +using easynav::RouteSegment; +using easynav::NavState; +using easynav::Costmap2D; + +class RoutesCostmapFilterTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) +{ + auto node = std::make_shared("routes_costmap_filter_missing"); + + RoutesCostmapFilter filter; + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); + + NavState nav_state; + // No 'routes' and no 'map' keys -> update should not crash + EXPECT_NO_THROW(filter.update(nav_state)); +} + +TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) +{ + auto node = std::make_shared("routes_costmap_filter_basic"); + + RoutesCostmapFilter filter; + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); + + // Simple costmap 10x1, resolution 1.0, origin at (0,0) + Costmap2D map(10, 1, 1.0, 0.0, 0.0); + // Initialize all costs to 0 + for (unsigned int x = 0; x < 10; ++x) { + map.setCost(x, 0, 0); + } + + // Single route from (2,0) to (7,0) + RoutesMap routes; + RouteSegment seg; + seg.id = "route0"; + seg.start.position.x = 2.0; + seg.start.position.y = 0.0; + seg.start.orientation.w = 1.0; + seg.end.position.x = 7.0; + seg.end.position.y = 0.0; + seg.end.orientation.w = 1.0; + routes.push_back(seg); + + NavState nav_state; + nav_state.set("routes", routes); + nav_state.set("map", map); + + filter.update(nav_state); + + auto map_after = nav_state.get("map"); + + // Cells clearly along the interior of the route should remain 0, + // others should be >= 50 (default min_cost). The last endpoint + // cell (x=7) may fall outside the corridor depending on the + // distance test, so we only assert [2,6] as "on route" here. + for (unsigned int x = 0; x < 10; ++x) { + if (x >= 2 && x <= 6) { + EXPECT_EQ(map_after.getCost(x, 0), 0) << "Cell on route changed at x=" << x; + } else { + EXPECT_GE(map_after.getCost(x, 0), 50) << "Cell outside route not raised at x=" << x; + } + } +} + +TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) +{ + auto node = std::make_shared("routes_costmap_filter_outside"); + + RoutesCostmapFilter filter; + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); + + // Costmap 5x1 from x=0..5 + Costmap2D map(5, 1, 1.0, 0.0, 0.0); + for (unsigned int x = 0; x < 5; ++x) { + map.setCost(x, 0, 0); + } + + // Route completely outside map, e.g., from (10,0) to (11,0) + RoutesMap routes; + RouteSegment seg; + seg.id = "route_outside"; + seg.start.position.x = 10.0; + seg.start.position.y = 0.0; + seg.start.orientation.w = 1.0; + seg.end.position.x = 11.0; + seg.end.position.y = 0.0; + seg.end.orientation.w = 1.0; + routes.push_back(seg); + + NavState nav_state; + nav_state.set("routes", routes); + nav_state.set("map", map); + + filter.update(nav_state); + + auto map_after = nav_state.get("map"); + + // No cell should be considered "on route"; all should be raised to >= 50 + for (unsigned int x = 0; x < 5; ++x) { + EXPECT_GE(map_after.getCost(x, 0), 50) << "Cell not raised at x=" << x; + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp new file mode 100644 index 00000000..9655d56b --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -0,0 +1,224 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using easynav::RoutesMapsManager; +using easynav::RoutesMap; +using easynav::RouteSegment; + +class RoutesMapsManagerTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +// Helper to create a temporary YAML file with given contents +static std::string create_temp_yaml(const std::string & contents) +{ + char filename[] = "/tmp/routes_test_XXXXXX.yaml"; + int fd = mkstemps(filename, 5); // keep .yaml suffix + if (fd == -1) { + throw std::runtime_error("Unable to create temp file"); + } + close(fd); + + std::ofstream out(filename); + out << contents; + out.close(); + return std::string(filename); +} + +TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node"); + + const std::string yaml = + "routes: [route1, route2]\n" + "route1:\n" + " start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + " end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + "route2:\n" + " start: {x: 1.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + " end: {x: 2.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + + const auto filename = create_temp_yaml(yaml); + + // Declare parameters expected by RoutesMapsManager + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters( + { + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 2u); + + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); + EXPECT_DOUBLE_EQ(routes[1].start.position.y, 1.0); + EXPECT_DOUBLE_EQ(routes[1].end.position.x, 2.0); +} + +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_empty_path"); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters( + { + rclcpp::Parameter("routes.map_path_file", std::string("")), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].start.position.y, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.y, 0.0); +} + +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_missing"); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("/tmp/non_existent_routes.yaml")); + + node->set_parameters( + { + rclcpp::Parameter("routes.map_path_file", std::string("/tmp/non_existent_routes.yaml")), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); +} + +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_no_routes_key"); + + const std::string yaml = "foo: bar\n"; + const auto filename = create_temp_yaml(yaml); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters( + { + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); +} + +TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_update"); + + const std::string yaml = + "routes: [route1]\n" + "route1:\n" + " start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + " end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + + const auto filename = create_temp_yaml(yaml); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters( + { + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + easynav::NavState nav_state; + manager->update(nav_state); + + ASSERT_TRUE(nav_state.has("routes")); + const auto & routes = nav_state.get("routes"); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst index bc658d4e..7e5d07a8 100644 --- a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Navstate key among filters are always map, not an arbitrary key +* Reset pose from RViz2 for every localizer +* Remove group points in tests +* Adaptations to `#94 `_ +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt index 8d7be651..6913465f 100644 --- a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt @@ -5,16 +5,13 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(nav_msgs REQUIRED) find_package(easynav_simple_common REQUIRED) find_package(std_srvs REQUIRED) find_package(yaets REQUIRED) @@ -59,6 +56,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + find_package(easynav_sensors REQUIRED) + find_package(rclcpp REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) add_subdirectory(tests) endif() diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 413125aa..00755612 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -1,23 +1,27 @@ # easynav_simple_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Simple Maps Manager that demonstrates the Maps Manager API. It forwards/republishes a basic occupancy map flow and exposes standard topics and a save-map service. At the heart of this stack is the SimpleMap data structure. It represents the environment as a 2D occupancy grid where each cell can be either 0 (free), 1 (occupied), or -1 (unknown). ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_simple_maps_manager/SimpleMapsManager` - **Type:** `easynav::SimpleMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -25,38 +29,42 @@ At the heart of this stack is the SimpleMap data structure. It represents the en - **Description:** Simple (no-op) Maps Manager that demonstrates the Maps Manager API. It forwards/republishes a basic occupancy map flow and exposes standard topics and a save-map service. ## Parameters -### Plugin Parameters (namespace: `//easynav_simple_maps_manager/OctomapMapsManager/...`) + +### Plugin Parameters (namespace: `//easynav_simple_maps_manager/SimpleMapsManager/...`) + | Name | Type | Default | Description | |---|---|---:|---| | `.package` | `string` | `""` | Package name used to resolve relative map paths via `ament_index`. | | `.map_path_file` | `string` | `""` | Relative path (inside the package) to a simple map. | - -### ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_map` | `nav_msgs/msg/OccupancyGrid` | Incoming occupancy map (used to feed the dynamic map). | QoS depth=1, transient_local, reliable | | Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Published static map. | QoS depth=1 | | Publisher | `//dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Published dynamic (live) map. | QoS depth=100 | - ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves current map(s) to disk. | - ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Optional perception points bundle (not strictly required for this no-op manager). | - +| `map.base` | `SimpleMap` | **Write** | Static map loaded from file / parameter configuration. | +| `map` | `SimpleMap` | **Write** | Dynamic map after applying incoming updates. | ## TF Frames + This manager does not broadcast TF; messages are stamped directly with their frame as provided. ## License -GPL-3.0-only + +Apache-2.0 diff --git a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp index ff543893..b91a0186 100644 --- a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp +++ b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the SimpleMapsManager method. @@ -23,24 +19,12 @@ #ifndef EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ #define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ -#include -#include -#include -#include -#include -#include - #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_simple_common/SimpleMap.hpp" -#include "yaets/tracing.hpp" - namespace easynav { @@ -70,9 +54,9 @@ class SimpleMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 15095e77..8c0c2c2f 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,10 +2,10 @@ easynav_simple_maps_manager - 0.0.2 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake @@ -24,6 +24,9 @@ ament_lint_auto ament_lint_common ament_cmake_gtest + easynav_sensors + rclcpp + rclcpp_lifecycle ament_cmake diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index ad724551..219b34b7 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -1,29 +1,23 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the SimpleMapsManager class. -#include #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -51,7 +45,7 @@ SimpleMapsManager::~SimpleMapsManager() } -std::expected +void SimpleMapsManager::on_initialize() { auto node = get_node(); @@ -70,39 +64,42 @@ SimpleMapsManager::on_initialize() try { pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; - } catch(ament_index_cpp::PackageNotFoundError & ex) { - return std::make_unexpected("Package " + package_name + " not found. Error: " + ex.what()); + } catch (ament_index_cpp::PackageNotFoundError & ex) { + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (!static_map_.load_from_file(map_path_)) { - return std::make_unexpected("File [" + map_path_ + "] not found"); + throw std::runtime_error("File [" + map_path_ + "] not found"); } } static_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); dynamic_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/dynamic_map", 100); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); incoming_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), - [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { + [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { static_map_.from_occupancy_grid(*msg); dynamic_map_.from_occupancy_grid(*msg); static_map_.to_occupancy_grid(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = this->get_node()->now(); static_occ_pub_->publish(static_grid_msg_); }); savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", - [this]( + node->get_name() + std::string("/") + plugin_name + "/savemap", + [&]( const std::shared_ptr request, std::shared_ptr response) { @@ -117,12 +114,10 @@ SimpleMapsManager::on_initialize() }); static_map_.to_occupancy_grid(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); - - return {}; } void @@ -144,19 +139,26 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.deep_copy(static_map_); - if (!nav_state.has("points")) { - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + } + + if (perceptions.empty()) { + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); return; } - const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + rclcpp::Time stamp; + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(dynamic_map_.resolution()) + .fuse(tf_info.map_frame, stamp, false) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(dynamic_map_.resolution()) - .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .as_points(); + const auto & fused = view.as_points(); for (const auto & p : fused) { if (dynamic_map_.check_bounds_metric(p.x, p.y)) { @@ -165,12 +167,12 @@ SimpleMapsManager::update(NavState & nav_state) } } - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); dynamic_map_.to_occupancy_grid(dynamic_grid_msg_); - dynamic_grid_msg_.header.frame_id = get_tf_prefix() + "map"; - dynamic_grid_msg_.header.stamp = get_node()->now(); + dynamic_grid_msg_.header.frame_id = tf_info.map_frame; + dynamic_grid_msg_.header.stamp = stamp; dynamic_occ_pub_->publish(dynamic_grid_msg_); } diff --git a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt index 003f5ef5..6f74618f 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt @@ -1,2 +1,7 @@ ament_add_gtest(simple_mapsmanager_tests simple_mapsmanager_tests.cpp) -target_link_libraries(simple_mapsmanager_tests ${PROJECT_NAME}) +target_link_libraries(simple_mapsmanager_tests + ${PROJECT_NAME} + easynav_sensors::easynav_sensors + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 858a99cb..0287e0c3 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -1,32 +1,28 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "tf2_ros/transform_listener.hpp" #include "std_srvs/srv/trigger.hpp" @@ -40,18 +36,6 @@ class SimpleMapsManagerTest : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - - easynav::NavState::register_printer( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override @@ -66,6 +50,13 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) { auto node = std::make_shared("test_node"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test"); auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock()); @@ -77,7 +68,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) easynav::NavState navstate; auto perception = std::make_shared(); - navstate.set("points", easynav::PointPerceptions()); + navstate.set("laser", easynav::PointPerception()); perception->data.points.resize(6); perception->data.points[0].x = 1.0; @@ -103,14 +94,13 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) perception->frame_id = "map"; perception->valid = true; - easynav::PointPerceptions perceptions; - perceptions.push_back(perception); - navstate.set("points", perceptions); + navstate.set("laser", perception); + // navstate.set_group("points", {"laser"}); manager->update(navstate); - ASSERT_TRUE(navstate.has("map.dynamic")); - auto map = navstate.get("map.dynamic"); + ASSERT_TRUE(navstate.has("map")); + auto map = navstate.get("map"); auto cell1 = map.metric_to_cell(1.0, 1.0); EXPECT_TRUE(map.at(cell1.first, cell1.second)); @@ -124,6 +114,13 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) { auto node = std::make_shared("test_node2"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test2"); easynav::NavState navstate; @@ -136,7 +133,7 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; + grid.header.frame_id = tf_info.map_frame; grid.info.width = 10; grid.info.height = 10; grid.info.resolution = 0.2; @@ -151,14 +148,15 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) manager->update(navstate); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ASSERT_TRUE(navstate.has("map.static")); - const auto & map = navstate.get("map.static"); + ASSERT_TRUE(navstate.has("map.base")); + const auto & map = navstate.get("map.base"); EXPECT_EQ(map.at(5, 5), 1); EXPECT_EQ(map.at(1, 1), 0); } -class FriendSimpleMapsManager : public easynav::SimpleMapsManager { +class FriendSimpleMapsManager : public easynav::SimpleMapsManager +{ public: void force_path(const std::string & path) {map_path_ = path;} }; @@ -167,6 +165,13 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) { auto node = std::make_shared("test_savemap_node"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test_savemap"); easynav::SimpleMap map_static; diff --git a/planners/easynav_costmap_planner/CHANGELOG.rst b/planners/easynav_costmap_planner/CHANGELOG.rst index 17d80b4d..f3cdd8f3 100644 --- a/planners/easynav_costmap_planner/CHANGELOG.rst +++ b/planners/easynav_costmap_planner/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package easynav_costmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Sync with Rolling branch +* Cells cost have no real impact in path creation +* Navstate key among filters are always map, not an arbitrary key +* Cells cost have no real impact in path creation +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Jose Miguel, José Miguel Guerrero, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/planners/easynav_costmap_planner/CMakeLists.txt b/planners/easynav_costmap_planner/CMakeLists.txt index 77ef543f..580b6d71 100644 --- a/planners/easynav_costmap_planner/CMakeLists.txt +++ b/planners/easynav_costmap_planner/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_costmap_common REQUIRED) diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index fff2666f..df142bd3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -1,6 +1,6 @@ # easynav_costmap_planner -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) +[![ROS 2: humble](https://img.shields.io/badge/ROS%202-humble-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description A planner plugin implementing a standard **A\*** path planner over a `Costmap2D` representation. It reads the dynamic costmap, goal list, and robot pose from NavState and publishes a computed path as a `nav_msgs/Path` message. @@ -12,6 +12,8 @@ A planner plugin implementing a standard **A\*** path planner over a `Costmap2D` ## Supported ROS 2 Distributions | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | @@ -29,12 +31,54 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | Name | Type | Default | Description | |---|---|---:|---| -| `.cost_factor` | `double` | `2.0` | Scaling factor applied to costmap cell values to compute traversal costs. | +| `.cost_factor` | `double` | `2.0` | Scaling factor applied to costmap cell values to compute traversal costs. Higher values make high-cost cells much less attractive. | | `.inflation_penalty` | `double` | `5.0` | Extra penalty added to inflated/near-obstacle cells to push paths away from obstacles. | -| `.cost_axial` | `double` | `1.0` | Base movement cost for axial (N/E/S/W) steps. | -| `.cost_diagonal` | `double` | `1.41` | Base movement cost for diagonal steps (approx. √2). | +| `.heuristic_scale` | `double` | `1.0` | Scaling factor applied to the A* heuristic term. Controls the trade-off between following the cheapest route vs. going more directly. | | `.continuous_replan` | `bool` | `true` | If `true`, re-plans continuously as the map/goal changes; if `false`, plans once per request. | +### Effect of `cost_factor` + +The planner uses a per-step traversal cost: + +$$ +g_{\text{new}} = g_{\text{current}} + \text{step\_cost} \cdot \left(1 + \text{cost\_factor} \cdot \frac{\text{cell\_cost}}{\text{LETHAL\_OBSTACLE}}\right) +$$ + +- **Low `cost_factor` (e.g. 1–2):** + - Cell cost has a modest influence; the planner is closer to a pure geometric shortest-path search. + - Paths may cross moderately expensive regions if that keeps distance short. +- **High `cost_factor` (e.g. 5–10+):** + - High-cost cells become very unattractive, so the planner strongly prefers low-cost corridors (e.g. routes or low-inflation areas), even if they are longer. + - Useful when combined with modules that encode preferences as higher costs (such as route managers). + +### Effect of `heuristic_scale` + +The A* priority is: + +$$ +f = g + h, \quad h = \text{heuristic\_scale} \cdot d_{\text{cells}}(\text{current}, \text{goal}) +$$ + +where $d_{\text{cells}}$ is the Euclidean distance in cell indices (not metric distance, but proportional). + +- **Decrease `heuristic_scale` (< 1.0):** + - The heuristic term becomes weaker, and A* behaves more like Dijkstra. + - The search explores more alternatives and is guided more strictly by the true accumulated cost $g$. + - This usually makes the planner follow high-cost penalties (e.g. for leaving a route) more faithfully, at the expense of more computation. + +- **Increase `heuristic_scale` (> 1.0):** + - The heuristic term dominates more, so the planner prefers geometrically direct paths. + - It may still avoid extremely high-cost regions if `cost_factor` is large, but will be more willing to “cut corners” through slightly more expensive zones. + +**Typical tuning strategy:** + +- First, adjust `cost_factor` so that the costmap properly encodes your preferences: + - Increase it until paths clearly avoid high-cost areas that should be disfavored. +- Then, fine-tune `heuristic_scale`: + - Start from `1.0`. + - If the planner still takes shortcuts that ignore cost differences, consider reducing it slightly (e.g. `0.7–0.9`). + - If planning becomes too slow or explores too widely, you can increase it modestly (e.g. `1.2–1.5`). + --- ## Interfaces (Topics and Services) @@ -52,7 +96,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Goal list used as planner targets. | -| `map.dynamic` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | +| `map` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose used as the start position. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | @@ -66,4 +110,4 @@ This plugin does not create subscriptions or services directly; it reads inputs --- ## License -GPL-3.0-only +Apache-2.0 diff --git a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp index c1bd7aee..67de89db 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the CostmapPlanner class implementing A* path planning using Costmap2D. @@ -23,7 +19,6 @@ #ifndef EASYNAV_COSTMAP_PLANNER__COSTMAPPLANNER_HPP_ #define EASYNAV_COSTMAP_PLANNER__COSTMAPPLANNER_HPP_ -#include #include #include "nav_msgs/msg/path.hpp" @@ -55,9 +50,9 @@ class CostmapPlanner : public PlannerMethodBase * Loads planner parameters, sets up ROS publishers, * and prepares the costmap-based planning environment. * - * @return std::expected A success indicator or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. @@ -71,8 +66,7 @@ class CostmapPlanner : public PlannerMethodBase protected: double cost_factor_; ///< Scaling factor applied to cell cost values. double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. - double cost_axial_; ///< Cost multiplier for axial (horizontal/vertical) moves. - double cost_diagonal_; ///< Cost multiplier for diagonal moves. + double heuristic_scale_ {1.0}; ///< Scaling factor applied to the heuristic term in A*. bool continuous_replan_ {true}; ///< Wheter replan path at freq time nav_msgs::msg::Path current_path_; ///< Most recently computed path. geometry_msgs::msg::Pose current_goal_; ///< Current goal. diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 053b985f..d309dc59 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -2,10 +2,10 @@ easynav_costmap_planner - 0.0.2 + 0.4.0 Easy Navigation: Costmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 2bed8add..796c0ba6 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the CostmapPlanner class using A* on Costmap2D. @@ -23,10 +19,10 @@ #include #include #include -#include #include #include "easynav_costmap_planner/CostmapPlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -69,66 +65,121 @@ static double compute_path_length(const nav_msgs::msg::Path & path) return total_length; } +// Simple path smoother: moving average over a sliding window in XY. +// Keeps endpoints unchanged to preserve exact start and goal. +static void smooth_path(std::vector & poses, int window_size = 5) +{ + if (poses.size() < 3 || window_size <= 1) { + return; + } + + // Ensure window_size is odd and at least 3 + if (window_size < 3) { + window_size = 3; + } + if (window_size % 2 == 0) { + window_size += 1; + } + + const int half = window_size / 2; + const size_t n = poses.size(); + std::vector original = poses; + + // Leave first and last pose untouched + for (size_t i = 1; i + 1 < n; ++i) { + double sum_x = 0.0; + double sum_y = 0.0; + int count = 0; + + const int begin = static_cast(std::max( + 0, + i > static_cast(half) ? i - half : 0)); + const int end = static_cast(std::min(n - 1, i + half)); + + for (int j = begin; j <= end; ++j) { + sum_x += original[j].position.x; + sum_y += original[j].position.y; + ++count; + } + + if (count > 0) { + poses[i].position.x = sum_x / static_cast(count); + poses[i].position.y = sum_y / static_cast(count); + } + } +} + CostmapPlanner::CostmapPlanner() { NavState::register_printer( [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length " + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); } -std::expected CostmapPlanner::on_initialize() +void CostmapPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); node->declare_parameter(plugin_name + ".cost_factor", 2.0); node->declare_parameter(plugin_name + ".inflation_penalty", 5.0); - node->declare_parameter(plugin_name + ".cost_axial", 1.0); - node->declare_parameter(plugin_name + ".cost_diagonal", 1.41); + node->declare_parameter(plugin_name + ".heuristic_scale", 1.0); node->declare_parameter(plugin_name + ".continuous_replan", true); node->get_parameter(plugin_name + ".cost_factor", cost_factor_); node->get_parameter(plugin_name + ".inflation_penalty", inflation_penalty_); - node->get_parameter(plugin_name + ".cost_axial", cost_axial_); - node->get_parameter(plugin_name + ".cost_diagonal", cost_diagonal_); - node->get_parameter(plugin_name + ".cost_diagonal", cost_diagonal_); + node->get_parameter(plugin_name + ".heuristic_scale", heuristic_scale_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); path_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; + node->get_name() + std::string("/") + plugin_name + "/path", 10); } void CostmapPlanner::update(NavState & nav_state) { - current_path_.poses.clear(); - if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map")) { return; } - const auto goals = nav_state.get("goals"); + const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { nav_state.set("path", current_path_); return; } - const auto & map = nav_state.get("map.dynamic"); - const auto robot_pose = nav_state.get("robot_pose"); + const auto & map = nav_state.get("map"); + const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (goals.header.frame_id != get_tf_prefix() + "map") { - RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", - goals.header.frame_id.c_str()); + rclcpp::Time latest_stamp = nav_state.get("map_time"); + if (rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { + latest_stamp = rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()); + } + if (rclcpp::Time( + goals.goals.front().header.stamp, + latest_stamp.get_clock_type()) > latest_stamp) + { + latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); + } + current_path_.header.stamp = latest_stamp; + + if (goals.header.frame_id != tf_info.map_frame) { + RCLCPP_WARN( + get_node()->get_logger(), "Goals frame is not 'map': %s", + goals.header.frame_id.c_str()); return; } unsigned int gx, gy; if (!map.worldToMap(goal.position.x, goal.position.y, gx, gy)) { - RCLCPP_WARN(get_node()->get_logger(), "Goal (%.2f, %.2f) is outside the map", goal.position.x, - goal.position.y); + RCLCPP_WARN( + get_node()->get_logger(), "Goal (%.2f, %.2f) is outside the map", goal.position.x, + goal.position.y); return; } @@ -142,8 +193,46 @@ void CostmapPlanner::update(NavState & nav_state) current_goal_ = goal; + // Lightweight skip: if inputs unchanged, avoid recomputation for a short window + static int last_sx = -1; + static int last_sy = -1; + static geometry_msgs::msg::Pose last_goal_pose; + static rclcpp::Time last_plan_time; + + unsigned int sx_chk, sy_chk; + if (map.worldToMap( + robot_pose.pose.pose.position.x, robot_pose.pose.pose.position.y, sx_chk, + sy_chk)) + { + const bool same_start_cell = (static_cast(sx_chk) == last_sx) && + (static_cast(sy_chk) == last_sy); + const bool same_goal_pose = ( + std::fabs(goal.position.x - last_goal_pose.position.x) < 1e-6 && + std::fabs(goal.position.y - last_goal_pose.position.y) < 1e-6 && + goal.orientation.x == last_goal_pose.orientation.x && + goal.orientation.y == last_goal_pose.orientation.y && + goal.orientation.z == last_goal_pose.orientation.z && + goal.orientation.w == last_goal_pose.orientation.w); + + // Initialize last_plan_time on first use to current node time + if (last_plan_time.nanoseconds() == 0) { + last_plan_time = get_node()->now(); + } + const double since_last = (get_node()->now() - last_plan_time).seconds(); + // Only allow skipping when continuous_replan_ is disabled (event-based planning) + if (!continuous_replan_ && same_start_cell && same_goal_pose && since_last < 0.05) { + // Skip re-planning when nothing relevant changed recently + nav_state.set("path", current_path_); + return; + } + } + auto poses = a_star_path(map, robot_pose.pose.pose, goal); if (!poses.empty()) { + // Apply a light smoothing to the raw grid path + smooth_path(poses); + + current_path_.poses.clear(); current_path_.header.stamp = get_node()->now(); current_path_.header.frame_id = goals.header.frame_id; for (const auto & pose : poses) { @@ -153,9 +242,18 @@ void CostmapPlanner::update(NavState & nav_state) pose_stamped.pose = pose; current_path_.poses.push_back(pose_stamped); } + // Always publish a newly computed path if (path_pub_->get_subscription_count() > 0) { path_pub_->publish(current_path_); } + // Update last inputs snapshot + unsigned int sx, sy; + if (map.worldToMap(robot_pose.pose.pose.position.x, robot_pose.pose.pose.position.y, sx, sy)) { + last_sx = static_cast(sx); + last_sy = static_cast(sy); + } + last_goal_pose = goal; + last_plan_time = get_node()->now(); } nav_state.set("path", current_path_); } @@ -170,13 +268,23 @@ std::vector CostmapPlanner::a_star_path( if (!map.worldToMap(goal.position.x, goal.position.y, gx, gy)) {return {};} int width = map.getSizeInCellsX(); + // Precompute constants used inside the neighbor loop + const double axial_cost = 1.0; + const double diagonal_cost = std::sqrt(2.0); auto idx = [&](int x, int y) {return y * width + x;}; std::priority_queue, std::greater<>> open; - std::unordered_map> came_from; - std::unordered_map cost_so_far; - open.push(GridNode{static_cast(sx), static_cast(sy), 0.0, heuristic(sx, sy, gx, gy)}); + const int height = map.getSizeInCellsY(); + const int total_cells = width * height; + std::vector parent_x(total_cells, -1); + std::vector parent_y(total_cells, -1); + std::vector cost_so_far(total_cells, std::numeric_limits::infinity()); + + const double initial_h = heuristic( + static_cast(sx), static_cast(sy), + static_cast(gx), static_cast(gy)) * heuristic_scale_; + open.push(GridNode{static_cast(sx), static_cast(sy), 0.0, initial_h}); cost_so_far[idx(sx, sy)] = 0.0; while (!open.empty()) { @@ -190,30 +298,30 @@ std::vector CostmapPlanner::a_star_path( if (!map.inBounds(nx, ny)) {continue;} uint8_t cell_cost = map.getCost(nx, ny); - if (cell_cost >= LETHAL_OBSTACLE) {continue;} + // Reject cells that would cause collision (>= INSCRIBED_INFLATED_OBSTACLE = 253) + if (cell_cost >= INSCRIBED_INFLATED_OBSTACLE) {continue;} - double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost) / LETHAL_OBSTACLE; - if (cell_cost >= INSCRIBED_INFLATED_OBSTACLE) { - double inflation_ratio = static_cast(cell_cost - INSCRIBED_INFLATED_OBSTACLE) / - (LETHAL_OBSTACLE - INSCRIBED_INFLATED_OBSTACLE); - traversal_cost += inflation_penalty_ * inflation_ratio; - } + // Calculate traversal cost: cost_factor_ acts as a direct multiplier on cell cost + double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost); - double step_cost = (dx == 0 || dy == 0) ? cost_axial_ : cost_diagonal_; + double step_cost = (dx == 0 || dy == 0) ? axial_cost : diagonal_cost; double new_cost = cost_so_far[idx(current.x, current.y)] + traversal_cost * step_cost; int nid = idx(nx, ny); - if (!cost_so_far.contains(nid) || new_cost < cost_so_far[nid]) { + if (new_cost < cost_so_far[nid]) { cost_so_far[nid] = new_cost; - open.push(GridNode{nx, ny, new_cost, new_cost + heuristic(nx, ny, gx, gy)}); - came_from[nid] = {current.x, current.y}; + const double h = heuristic(nx, ny, static_cast(gx), static_cast(gy)) * + heuristic_scale_; + open.push(GridNode{nx, ny, new_cost, new_cost + h}); + parent_x[nid] = current.x; + parent_y[nid] = current.y; } } } std::vector path; int cx = static_cast(gx), cy = static_cast(gy); - while (came_from.contains(idx(cx, cy))) { + while (parent_x[idx(cx, cy)] != -1) { double wx, wy; map.mapToWorld(cx, cy, wx, wy); geometry_msgs::msg::Pose pose; @@ -221,7 +329,10 @@ std::vector CostmapPlanner::a_star_path( pose.position.y = wy; pose.orientation = goal.orientation; path.push_back(pose); - std::tie(cx, cy) = came_from[idx(cx, cy)]; + int px = parent_x[idx(cx, cy)]; + int py = parent_y[idx(cx, cy)]; + cx = px; + cy = py; } std::reverse(path.begin(), path.end()); diff --git a/planners/easynav_navmap_planner/CHANGELOG.rst b/planners/easynav_navmap_planner/CHANGELOG.rst index bb10c0b2..68008d52 100644 --- a/planners/easynav_navmap_planner/CHANGELOG.rst +++ b/planners/easynav_navmap_planner/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package easynav_navmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Sync with Rolling branch +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/planners/easynav_navmap_planner/CMakeLists.txt b/planners/easynav_navmap_planner/CMakeLists.txt index 950f854d..6e0ec154 100644 --- a/planners/easynav_navmap_planner/CMakeLists.txt +++ b/planners/easynav_navmap_planner/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 58d80e1b..6f5f891e 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -1,9 +1,29 @@ # easynav_navmap_planner -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) +[![ROS 2: humble](https://img.shields.io/badge/ROS%202-humble-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description -A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goals from NavState and publishes a `nav_msgs/Path`. +**A\*** path planner operating over a `NavMap` triangular mesh. +The planner computes the **minimum-cost path** between the robot and the goal, taking into account both the geometric distance between triangles and the cost values stored in a selected NavMap layer (typically `"inflated_obstacles"`). + +Instead of simply avoiding non-free NavCels, the planner integrates their cost values (0–255) into the path evaluation. Cells marked as `LETHAL_OBSTACLE` or `NO_INFORMATION` are considered non-traversable, while inflated or inscribed cells are allowed but penalized proportionally to their cost. + +This enables smoother and safer trajectories that still respect proximity constraints imposed by obstacle inflation. + + +### Cost model +For two neighboring NavCels `u` and `v`, the edge cost is computed as: + +\[ +\text{cost}(u,v) = d(u,v) \times \left(\text{cost\_factor} + \text{inflation\_penalty} \times \frac{c(v)}{253}\right) +\] + +where `d(u,v)` is the Euclidean distance between triangle centroids, +and `c(v)` is the cost value of cell `v`. +This formulation ensures that: +- cells near obstacles (high cost) are traversed only if geometrically necessary, +- lethal (`254`) and unknown (`255`) cells are not traversable. + ## Authors and Maintainers - **Authors:** Intelligent Robotics Lab @@ -12,6 +32,8 @@ A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goal ## Supported ROS 2 Distributions | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | @@ -20,18 +42,18 @@ A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goal - **Type:** `easynav::navmap::AStarPlanner` - **Base Class:** `easynav::PlannerMethodBase` - **Library:** `easynav_navmap_planner` -- **Description:** A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goals from NavState and publishes a `nav_msgs/Path`. +- **Description:** A\* path planner over a NavMap triangular mesh using per-cell costs to compute the shortest safe path. ## Parameters -All parameters are declared under the plugin namespace, i.e., `//easynav_navmap_planner/AStarPlanner/...`. +All parameters are declared under the plugin namespace, i.e. +`//easynav_navmap_planner/AStarPlanner/...` | Name | Type | Default | Description | |---|---|---:|---| -| `.layer` | `string` | `"inflated_obstacles"` | NavMap layer name to read costs from (e.g., `inflated_obstacles`). | -| `.cost_factor` | `double` | `2.0` | Scaling factor applied to cell/triangle costs. | -| `.inflation_penalty` | `double` | `5.0` | Extra penalty near inflated/inscribed regions to keep paths away from obstacles. | -| `.continuous_replan` | `bool` | `true` | Replan continuously as NavState updates (true) or plan once per request (false). | +| `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | +| `.continuous_replan` | `bool` | `true` | If true, recomputes the path whenever `NavState` updates; if false, plans once per goal. | +**Note:** The planner internally uses hardcoded values for `layer_name` (prefers `"inflated_obstacles"`, fallback to `"obstacles"`), `inflation_penalty` (value used in cost calculation), `cost_axial`, and `cost_diagonal`. These are not runtime-configurable parameters. ## Interfaces (Topics and Services) @@ -40,20 +62,18 @@ All parameters are declared under the plugin namespace, i.e., `//easyn |---|---|---|---|---| | Publisher | `//path` | `nav_msgs/msg/Path` | Publishes the computed A* path. | depth=10 | - -This plugin does not create subscriptions or services directly; it reads inputs from `NavState`. +This plugin does not create subscriptions or services directly; it retrieves all inputs from `NavState`. ## NavState Keys -| Key | Type | Access | Notes | +| Key | Type | Access | Description | |---|---|---|---| -| `goals` | `nav_msgs::msg::Goals` | **Read** | Planner targets. | -| `map` | `::navmap::NavMap` | **Read** | NavMap (reads the specified `layer`). | -| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start pose for path planning. | -| `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | - +| `goals` | `nav_msgs::msg::Goals` | **Read** | Target pose(s) for path planning. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose (start position). | +| `map.navmap` | `::navmap::NavMap` | **Read** | NavMap containing geometry and cost layer. The planner reads costs from the layer specified in `.layer` (default: `"inflated_obstacles"`). If that layer does not exist, it automatically falls back to `"obstacles"`. | +| `path` | `nav_msgs::msg::Path` | **Write** | Output path, computed as the lowest-cost route. | ## TF Frames -This plugin does not perform TF lookups directly; frame consistency is assumed between NavMap, robot pose, and published path. +The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License -GPL-3.0-only +Apache-2.0 diff --git a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp index 09577ffc..3b0cc064 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the AStarPlanner class implementing A* path planning using ::navmap::NavMap. @@ -23,8 +19,9 @@ #ifndef EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ #define EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ -#include #include +#include +#include #include "nav_msgs/msg/path.hpp" @@ -39,7 +36,7 @@ namespace navmap /// \brief A planner implementing the A* algorithm on a ::navmap::NavMap grid. /// -/// This class generates a collision-free path using A* search over a 2D costmap. +/// This class generates a collision-free path using A* search over a surface-based NavMap. /// It supports cost-based penalties and anisotropic movement costs. class AStarPlanner : public PlannerMethodBase { @@ -55,11 +52,11 @@ class AStarPlanner : public PlannerMethodBase * @brief Initializes the planner. * * Loads planner parameters, sets up ROS publishers, - * and prepares the costmap-based planning environment. + * and prepares the NavMap-based planning environment. * - * @return std::expected A success indicator or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. @@ -72,17 +69,37 @@ class AStarPlanner : public PlannerMethodBase protected: double cost_factor_; ///< Scaling factor applied to cell cost values. - double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. - double cost_axial_; ///< Cost multiplier for axial (horizontal/vertical) moves. - double cost_diagonal_; ///< Cost multiplier for diagonal moves. + double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. + double cost_axial_; ///< Cost multiplier for axial (horizontal/vertical) moves. + double cost_diagonal_; ///< Cost multiplier for diagonal moves. std::string layer_name_; - bool continuous_replan_ {true}; ///< Wheter replan path at freq time + bool continuous_replan_ {true}; ///< Whether to replan the path at control frequency. nav_msgs::msg::Path current_path_; ///< Most recently computed path. geometry_msgs::msg::Pose current_goal_; ///< Current goal. /// Publisher for the computed navigation path (for visualization or monitoring). rclcpp::Publisher::SharedPtr path_pub_; + /// Cached centroids for each NavCel (same indexing as ::navmap::NavMap::navcels). + std::vector centroids_; + + /// Cached per-NavCel occupancy / cost values (0..255). + std::vector occ_; + + /// Reusable buffers for A* search cost and parent links. + std::vector g_; + std::vector<::navmap::NavCelId> parent_; + + /** + * @brief Ensure internal caches (centroids and A* buffers) are sized for the given map. + * + * This avoids reallocations on every planning call. Values in g_ and parent_ + * are reset for the current run. + * + * @param map The NavMap for which caches must be valid. + */ + void ensure_graph_cache(const ::navmap::NavMap & map); + /** * @brief Smooth a Path in XY while keeping every waypoint inside its original NavCel. * @@ -114,27 +131,21 @@ class AStarPlanner : public PlannerMethodBase /** * @brief Internal A* path planning routine. * - * Computes a path on the given costmap from the start pose to the goal pose. + * Computes a path on the given NavMap from the start pose to the goal pose. * * Movement cost is influenced by: - * - The cost of each cell (retrieved from the costmap). + * - The cost of each NavCel (retrieved from a layer). * - Additional inflation penalties near obstacles. - * - Anisotropic weights for axial vs diagonal movement. * - * @param map The costmap to plan over. + * @param map The NavMap to plan over. * @param start The robot's starting pose in world coordinates. - * @param goal The goal pose in world coordinates. + * @param goal The goal pose in world coordinates. * @return A vector of poses representing the planned path. */ std::vector a_star_path( const ::navmap::NavMap & map, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal); - - /** - * @brief Internal static map. - */ - ::navmap::NavMap navmap_; }; } // namespace navmap diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 3376dd9e..872dc366 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -2,10 +2,10 @@ easynav_navmap_planner - 0.0.2 + 0.4.0 Easy Navigation: navmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 90a83264..30742d99 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -1,52 +1,41 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Implementation of the AStarPlanner class using A* on ::navmap::NavMap (triangle graph). #include -#include -#include #include #include -#include -#include #include +#include +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_navmap_planner/AStarPlanner.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" namespace easynav { namespace navmap { -static constexpr uint8_t NO_INFORMATION = 255; -static constexpr uint8_t LETHAL_OBSTACLE = 254; -static constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr uint8_t MAX_NON_OBSTACLE = 252; -static constexpr uint8_t FREE_SPACE = 0; - static double compute_path_length(const nav_msgs::msg::Path & path) { double total_length = 0.0; @@ -63,28 +52,26 @@ AStarPlanner::AStarPlanner() NavState::register_printer( [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length " + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); } -std::expected AStarPlanner::on_initialize() +void AStarPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); node->declare_parameter(plugin_name + ".cost_factor", 2.0); - node->declare_parameter(plugin_name + ".inflation_penalty", 5.0); node->declare_parameter(plugin_name + ".continuous_replan", true); node->get_parameter(plugin_name + ".cost_factor", cost_factor_); - node->get_parameter(plugin_name + ".inflation_penalty", inflation_penalty_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); path_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; + node->get_name() + std::string("/") + plugin_name + "/path", 10); } void AStarPlanner::update(NavState & nav_state) @@ -94,21 +81,22 @@ void AStarPlanner::update(NavState & nav_state) return; } - - const auto goals = nav_state.get("goals"); - if (goals.goals.empty()) { + const auto & goals = nav_state.get("goals"); + if (goals.goals.empty() || !nav_state.has("map.navmap")) { nav_state.set("path", current_path_); return; } - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); - const auto robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (goals.header.frame_id != get_tf_prefix() + "map") { - RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", - goals.header.frame_id.c_str()); + if (goals.header.frame_id != tf_info.map_frame) { + RCLCPP_WARN( + get_node()->get_logger(), "Goals frame is not 'map': %s", + goals.header.frame_id.c_str()); return; } @@ -121,7 +109,7 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = goal; - auto poses = a_star_path(navmap_, robot_pose.pose.pose, goal); + auto poses = a_star_path(navmap, robot_pose.pose.pose, goal); if (!poses.empty()) { current_path_.header.stamp = get_node()->now(); current_path_.header.frame_id = goals.header.frame_id; @@ -134,7 +122,7 @@ void AStarPlanner::update(NavState & nav_state) current_path_.poses.push_back(std::move(ps)); } - current_path_ = path_smoother(current_path_, navmap_); + current_path_ = path_smoother(current_path_, navmap); if (path_pub_->get_subscription_count() > 0) { path_pub_->publish(current_path_); @@ -143,7 +131,6 @@ void AStarPlanner::update(NavState & nav_state) nav_state.set("path", current_path_); } - nav_msgs::msg::Path AStarPlanner::path_smoother( const nav_msgs::msg::Path & in_path, @@ -168,9 +155,10 @@ AStarPlanner::path_smoother( // Fill pts from input and locate cids for (size_t i = 0; i < N; ++i) { const auto & p = out.poses[i].pose.position; - pts[i] = Eigen::Vector3f(static_cast(p.x), - static_cast(p.y), - static_cast(p.z)); + pts[i] = Eigen::Vector3f( + static_cast(p.x), + static_cast(p.y), + static_cast(p.z)); } // Use walking hints to speed up sequential location @@ -206,7 +194,8 @@ AStarPlanner::path_smoother( } // Helper to fetch triangle vertices (A,B,C) for a cid - auto get_triangle_vertices = [&](::navmap::NavCelId cid) -> std::array { + auto get_triangle_vertices = + [&](::navmap::NavCelId cid) -> std::array { const ::navmap::NavCel & tri = navmap.navcels[cid]; const auto A = navmap.positions.at(tri.v[0]); const auto B = navmap.positions.at(tri.v[1]); @@ -215,8 +204,8 @@ AStarPlanner::path_smoother( }; // Helper: clamp a 3D point to the triangle of a given cid (closest point) - auto clamp_to_triangle = [&](const Eigen::Vector3f & p, - ::navmap::NavCelId cid) -> Eigen::Vector3f { + auto clamp_to_triangle = + [&](const Eigen::Vector3f & p, ::navmap::NavCelId cid) -> Eigen::Vector3f { const auto V = get_triangle_vertices(cid); return ::navmap::closest_point_on_triangle(p, V[0], V[1], V[2]); }; @@ -271,7 +260,7 @@ AStarPlanner::path_smoother( // Clamp to the *original* triangle of this point Eigen::Vector3f clamped = clamp_to_triangle(cand3, cids[i]); - next[i] = clamped; // already lies on triangle plane, z' consistent + next[i] = clamped; // already lies on triangle plane, z' consistent } curr.swap(next); } @@ -287,114 +276,221 @@ AStarPlanner::path_smoother( return out; } +// Helper: detect if a layer exists (optional; if you already have API, adjust accordingly) +static inline bool layer_exists(const ::navmap::NavMap & nm, const std::string & name) +{ + return static_cast(nm.layers.get(name)); +} + +void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) +{ + const std::size_t N = map.navcels.size(); + + // Cache centroids: only recompute when the number of NavCels changes. + if (centroids_.size() != N) { + centroids_.resize(N); + for (::navmap::NavCelId c = 0; c < static_cast<::navmap::NavCelId>(N); ++c) { + const auto cc = map.navcel_centroid(c); + centroids_[c] = Eigen::Vector3f{cc.x(), cc.y(), cc.z()}; + } + } + + // Ensure occupancy buffer has the right size (values are filled per-planning call). + if (occ_.size() != N) { + occ_.resize(N); + } + + // Resize and reset A* buffers. + const double inf = std::numeric_limits::infinity(); + + if (g_.size() != N) { + g_.assign(N, inf); + } else { + std::fill(g_.begin(), g_.end(), inf); + } + + if (parent_.size() != N) { + parent_.assign(N, std::numeric_limits<::navmap::NavCelId>::max()); + } else { + std::fill( + parent_.begin(), parent_.end(), + std::numeric_limits<::navmap::NavCelId>::max()); + } +} + std::vector AStarPlanner::a_star_path( const ::navmap::NavMap & nm, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal) { using ::navmap::NavCelId; + using namespace navmap_ros; if (nm.navcels.empty()) {return {};} - // 1) Locate start & goal navcels (fallback to closest triangle) - size_t sidx_s = 0, sidx_g = 0; + // 1) Locate start and goal NavCels (fallback to closest triangle if necessary) + std::size_t sidx_s = 0, sidx_g = 0; NavCelId cid_start = 0, cid_goal = 0; - Eigen::Vector3f bary; Eigen::Vector3f hit; + Eigen::Vector3f bary; + Eigen::Vector3f hit; Eigen::Vector3f pS(start.position.x, start.position.y, start.position.z); Eigen::Vector3f pG(goal.position.x, goal.position.y, goal.position.z); bool okS = nm.locate_navcel(pS, sidx_s, cid_start, bary, &hit); if (!okS) { - Eigen::Vector3f q; float d2; + Eigen::Vector3f q; + float d2; if (!nm.closest_navcel(pS, sidx_s, cid_start, q, d2)) {return {};} } bool okG = nm.locate_navcel(pG, sidx_g, cid_goal, bary, &hit); if (!okG) { - Eigen::Vector3f q; float d2; + Eigen::Vector3f q; + float d2; if (!nm.closest_navcel(pG, sidx_g, cid_goal, q, d2)) {return {};} } - const size_t N = nm.navcels.size(); + const std::size_t N = nm.navcels.size(); - // 3) Precompute centroids (2D) for consistent metric and heuristic - std::vector C(N); - for (NavCelId c = 0; c < N; ++c) { - const auto cc = nm.navcel_centroid(c); - C[c] = {cc.x(), cc.y(), cc.z()}; - } + // 2) Choose cost layer: prefer "inflated_obstacles", fallback to "obstacles" + const std::string cost_layer = + layer_exists(nm, "inflated_obstacles") ? "inflated_obstacles" : "obstacles"; - auto h = [&](NavCelId a, NavCelId b) -> double { - const auto d = C[a] - C[b]; + // Ensure cached buffers match the current NavMap. + ensure_graph_cache(nm); + + // Precomputed centroids in `centroids_` are used for cost, heuristic, and edge lengths. + auto euclid = [&](NavCelId a, NavCelId b) -> double { + const auto d = centroids_[a] - centroids_[b]; return static_cast(d.norm()); }; + // Cache per-NavCel uint8_t cost values (0..255) for the selected layer. + for (NavCelId c = 0; c < static_cast(N); ++c) { + // If the cell has no stored value, assume FREE_SPACE (0). + occ_[c] = nm.layer_get(cost_layer, c, FREE_SPACE); + } + + // Traversability: block lethal and unknown. + auto traversable = [&](NavCelId c) -> bool { + const std::uint8_t v = occ_[c]; + return (v != LETHAL_OBSTACLE) && (v != NO_INFORMATION); + }; + + // Normalize a uint8_t cost into [0, 1]; FREE_SPACE=0 → 0.0; INSCRIBED=253 → ~1.0. + // Returns +inf for non-traversable (lethal/unknown). + auto normalized_cost = [&](NavCelId c) -> double { + const std::uint8_t v = occ_[c]; + if (v == LETHAL_OBSTACLE || v == NO_INFORMATION) { + return std::numeric_limits::infinity(); + } + // Cap at INSCRIBED_INFLATED_OBSTACLE to avoid division by 0 at 254/255. + const double max_cost = static_cast(INSCRIBED_INFLATED_OBSTACLE); // 253 + return static_cast(v) / max_cost; // FREE=0 → 0.0, INSCRIBED=253 → 1.0 + }; + + // If start or goal lands on non-traversable, do not plan. + if (!traversable(cid_start) || !traversable(cid_goal)) { + return {}; + } + + // Weighted step cost: + // base geometric cost (edge length) scaled by (cost_factor_ + inflation_penalty_ * norm_cost(target)). + // This preserves admissibility with heuristic h = Euclidean distance, since the minimal multiplier ≥ 1. auto step_cost = [&](NavCelId from, NavCelId to) -> double { - const double dist = static_cast((C[from] - C[to]).norm()); - return dist; + const double base = euclid(from, to); + if (!std::isfinite(base) || base <= 0.0) { + return std::numeric_limits::infinity(); + } + + const double ncost = normalized_cost(to); + if (!std::isfinite(ncost)) { + return std::numeric_limits::infinity(); + } + + // Ensure the multiplier is at least 1.0 so h = euclid remains admissible. + // If your cost_factor_ is already ≥ 1, this holds. Otherwise we clamp. + const double cf = std::max(1.0, static_cast(cost_factor_)); + const double mult = cf + static_cast(inflation_penalty_) * ncost; + + return base * mult; }; - // 4) A* on triangle graph - struct Node { NavCelId cid; double f; }; - struct Cmp { bool operator()(const Node & a, const Node & b) const {return a.f > b.f;} }; + // 4) A* search on the triangle graph, using cost-aware edges. + struct Node + { + NavCelId cid; + double f; + }; + struct Cmp + { + bool operator()(const Node & a, const Node & b) const {return a.f > b.f;} + }; std::priority_queue, Cmp> open; - std::vector g(N, std::numeric_limits::infinity()); - std::vector in_open(N, 0); - std::vector parent(N, std::numeric_limits::max()); - g[cid_start] = 0.0; + auto h = [&](NavCelId a, NavCelId b) -> double { + // Heuristic: pure Euclidean distance → admissible (never overestimates). + return euclid(a, b); + }; + + g_[cid_start] = 0.0; open.push(Node{cid_start, h(cid_start, cid_goal)}); - in_open[cid_start] = 1; while (!open.empty()) { - const auto cur = open.top(); open.pop(); + const auto cur = open.top(); + open.pop(); const NavCelId u = cur.cid; if (u == cid_goal) {break;} - // Optional: restrict to the goal surface (comment if you want cross-surface paths via explicit neighbors) - // const size_t surface_goal = sidx_g; - // if (surface_goal != sidx_s) { - // // do nothing special; graph neighbors already encode connectivity - // } + const auto & tri = nm.navcels[u]; + for (int e = 0; e < 3; ++e) { + NavCelId v = tri.neighbor[e]; + if (v == std::numeric_limits::max()) { + continue; + } + const std::size_t vidx = static_cast(v); + if (vidx >= N) { + continue; + } - for (NavCelId v : nm.navcel_neighbors(u)) { - const size_t vidx = static_cast(v); - if (vidx >= N) {continue;} + // Skip non-traversable neighbors (lethal or unknown). + if (!traversable(v)) {continue;} const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} - const double tentative = g[u] + sc; - if (tentative < g[v]) { - g[v] = tentative; - parent[v] = u; + const double tentative = g_[u] + sc; + if (tentative < g_[v]) { + g_[v] = tentative; + parent_[v] = u; const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); - in_open[v] = 1; } } } - if (!std::isfinite(g[cid_goal])) { + if (!std::isfinite(g_[cid_goal])) { return {}; } - // 5) Reconstruct path (centroidal polyline) + // 5) Path reconstruction (centroid-based polyline). std::vector path; - for (NavCelId c = cid_goal; c != std::numeric_limits::max(); c = parent[c]) { + for (NavCelId c = cid_goal; + c != std::numeric_limits::max(); + c = parent_[c]) + { geometry_msgs::msg::Pose p; - p.position.x = C[c].x(); - p.position.y = C[c].y(); - p.position.z = C[c].z(); + p.position.x = centroids_[c].x(); + p.position.y = centroids_[c].y(); + p.position.z = centroids_[c].z(); p.orientation = goal.orientation; path.push_back(std::move(p)); if (c == cid_start) {break;} } std::reverse(path.begin(), path.end()); - // Ensure at least goal pose if (path.empty()) {path.push_back(goal);} return path; } diff --git a/planners/easynav_simple_planner/CHANGELOG.rst b/planners/easynav_simple_planner/CHANGELOG.rst index 472ae955..8fa451f8 100644 --- a/planners/easynav_simple_planner/CHANGELOG.rst +++ b/planners/easynav_simple_planner/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package easynav_simple_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2026-07-26) +------------------ +* Sync with Rolling branch +* Navstate key among filters are always map, not an arbitrary key +* Navstate key among filters are always map, not an arbitrary key +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + +0.2.1 (2026-02-27) +------------------ +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/planners/easynav_simple_planner/CMakeLists.txt b/planners/easynav_simple_planner/CMakeLists.txt index 5d6975ce..0282ad71 100644 --- a/planners/easynav_simple_planner/CMakeLists.txt +++ b/planners/easynav_simple_planner/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_simple_common REQUIRED) diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index c19d8460..efe694cc 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -1,6 +1,6 @@ # easynav_simple_planner -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) +[![ROS 2: humble](https://img.shields.io/badge/ROS%202-humble-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description A simple A* planner over a lightweight `SimpleMap` grid. It reads the dynamic simple map, goals, and robot pose from NavState and publishes a `nav_msgs/Path`. @@ -12,6 +12,8 @@ A simple A* planner over a lightweight `SimpleMap` grid. It reads the dynamic si ## Supported ROS 2 Distributions | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | @@ -45,7 +47,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Planner targets. | -| `map.dynamic` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | +| `map` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start pose for path planning. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | @@ -54,4 +56,4 @@ This plugin does not create subscriptions or services directly; it reads inputs This plugin does not perform TF lookups directly; frame consistency is assumed between map, robot pose, and published path. ## License -GPL-3.0-only +Apache-2.0 diff --git a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp index 6dbb3253..985f782f 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -1,21 +1,17 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief Declaration of the SimplePlanner class implementing A* path planning. @@ -23,7 +19,6 @@ #ifndef EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ #define EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ -#include #include #include "nav_msgs/msg/path.hpp" @@ -52,9 +47,9 @@ class SimplePlanner : public PlannerMethodBase * Configures publishers, retrieves parameters, and prepares the planner * for path generation using the available map data. * - * @return std::expected Success or an error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the planner by computing a new path. diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index e092f111..5be10b4c 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -2,10 +2,10 @@ easynav_simple_planner - 0.0.2 + 0.4.0 Easy Navigation: Simple planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index d7ca5b8f..eaadb896 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -1,28 +1,24 @@ // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// Easy Navigation program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. +// http://www.apache.org/licenses/LICENSE-2.0 // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include #include -#include #include "easynav_simple_planner/SimplePlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -79,14 +75,15 @@ SimplePlanner::SimplePlanner() [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length" << + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); } -std::expected +void SimplePlanner::on_initialize() { auto node = get_node(); @@ -98,9 +95,7 @@ SimplePlanner::on_initialize() node->get_parameter(plugin_name + ".clearance_distance", clearance_distance_); path_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - - return {}; + node->get_name() + std::string("/") + plugin_name + "/path", 10); } void @@ -111,39 +106,51 @@ SimplePlanner::update(NavState & nav_state) if (!nav_state.has("goals")) {return;} if (!nav_state.has("robot_pose")) {return;} - const auto goals = nav_state.get("goals"); + const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { nav_state.set("path", current_path_); return; } - if (!nav_state.has("map.dynamic")) { - RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map.dynamic map not found"); + if (!nav_state.has("map")) { + RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map map not found"); return; } SimpleMap map_typed; - if (nav_state.has("map.dynamic")) { - map_typed = nav_state.get("map.dynamic"); + if (nav_state.has("map")) { + map_typed = nav_state.get("map"); } else { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.dynamic map"); + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map"); return; } - const auto robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + const auto clock_type = get_node()->get_clock()->get_clock_type(); + rclcpp::Time latest_stamp(robot_pose.header.stamp, clock_type); + if (rclcpp::Time( + goals.goals.front().header.stamp, + latest_stamp.get_clock_type()) > latest_stamp) + { + latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); + } auto downsampled_map = map_typed.downsample(0.2); - if (goals.header.frame_id != "map") { - RCLCPP_WARN(get_node()->get_logger(), + if (goals.header.frame_id != tf_info.map_frame) { + RCLCPP_WARN( + get_node()->get_logger(), "SimplePlanner::update goals frame is not map (%s)", goals.header.frame_id.c_str()); return; } if (!downsampled_map->check_bounds_metric(goal.position.x, goal.position.y)) { - RCLCPP_WARN(get_node()->get_logger(), + RCLCPP_WARN( + get_node()->get_logger(), "SimplePlanner::update goal (%lf, %lf) outside the map", goal.position.x, goal.position.y); return; } @@ -155,13 +162,13 @@ SimplePlanner::update(NavState & nav_state) downsampled_map->resolution()); if (!poses.empty()) { - current_path_.header.stamp = get_node()->now(); + current_path_.header.stamp = latest_stamp; current_path_.header.frame_id = goals.header.frame_id; for (const auto & pose : poses) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.frame_id = goals.header.frame_id; - pose_stamped.header.stamp = get_node()->now(); + pose_stamped.header.stamp = latest_stamp; pose_stamped.pose = pose; current_path_.poses.push_back(pose_stamped); } @@ -206,7 +213,8 @@ SimplePlanner::a_star_path( double resolution) { RCLCPP_DEBUG(get_node()->get_logger(), "Running A* ============"); - RCLCPP_DEBUG(get_node()->get_logger(), "Path from (%lf m, %lf m) -> (%lf m, %lf m)", + RCLCPP_DEBUG( + get_node()->get_logger(), "Path from (%lf m, %lf m) -> (%lf m, %lf m)", start.position.x, start.position.y, goal.position.x, goal.position.y); @@ -216,7 +224,8 @@ SimplePlanner::a_star_path( auto [sx, sy] = map.metric_to_cell(start.position.x, start.position.y); auto [gx, gy] = map.metric_to_cell(goal.position.x, goal.position.y); - RCLCPP_DEBUG(get_node()->get_logger(), "Path from (%d, %d) -> (%d, %d)", + RCLCPP_DEBUG( + get_node()->get_logger(), "Path from (%d, %d) -> (%d, %d)", sx, sy, gx, gy); std::priority_queue, std::greater> open; @@ -246,7 +255,7 @@ SimplePlanner::a_star_path( double new_cost = cost_so_far[idx(current.x, current.y)] + hypot(dx, dy); int nid = idx(nx, ny); - if (!cost_so_far.contains(nid) || new_cost < cost_so_far[nid]) { + if (cost_so_far.find(nid) == cost_so_far.end() || new_cost < cost_so_far[nid]) { cost_so_far[nid] = new_cost; double priority = new_cost + heuristic(nx, ny, gx, gy); open.push({nx, ny, new_cost, priority}); @@ -257,7 +266,7 @@ SimplePlanner::a_star_path( std::vector path; int cx = gx, cy = gy; - while (came_from.contains(idx(cx, cy))) { + while (came_from.find(idx(cx, cy) ) != came_from.end()) { geometry_msgs::msg::Pose pose; auto [px, py] = map.cell_to_metric(cx, cy); @@ -267,7 +276,8 @@ SimplePlanner::a_star_path( path.push_back(pose); - RCLCPP_DEBUG(get_node()->get_logger(), "\t(%d, %d) = (%lf m, %lf m)", + RCLCPP_DEBUG( + get_node()->get_logger(), "\t(%d, %d) = (%lf m, %lf m)", cx, cy, px, py); std::tie(cx, cy) = came_from[idx(cx, cy)];