diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..3b3df6d7 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,12 @@ +Checks: 'boost-*, + bugprone-*, + cert-*,-cert-err58-cpp, + clang-*,-clang-analyzer-core.uninitialized.Assign, + concurrency-*, + cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-avoid-magic-numbers,-cppcoreguidelines-init-variables, + misc-*, + modernize-*,-modernize-use-trailing-return-type,-modernize-use-nodiscard, + performance-*, + portability-*, + readability-*,-readability-magic-numbers' +FormatStyle: file diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 00000000..f755af2e --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,20 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: melodic} + - {ROS_DISTRO: noetic, CMAKE_ARGS: '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'} + fail-fast: false + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: 'ros-industrial/industrial_ci@master' + env: + ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }} + CMAKE_ARGS: ${{ matrix.env.CMAKE_ARGS }} + VERBOSE_TESTS: true diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..bb137b63 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.0.2) +add_compile_options(-std=c++17 -Wall -Wextra -Wpedantic -Werror) +project(path_tracking_pid) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED + COMPONENTS + actionlib + actionlib_msgs + dynamic_reconfigure + geometry_msgs + mbf_costmap_core + mbf_msgs + message_generation + nav_msgs + pluginlib + roscpp + roslint + rostest + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs +) + +add_message_files( + FILES + PidDebug.msg + PidFeedback.msg +) + +generate_dynamic_reconfigure_options( + cfg/Pid.cfg +) + +generate_messages( + DEPENDENCIES + actionlib_msgs + geometry_msgs + nav_msgs + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + actionlib + actionlib_msgs + dynamic_reconfigure + geometry_msgs + mbf_costmap_core + mbf_msgs + message_runtime + nav_msgs + pluginlib + roscpp + std_msgs + tf2_geometry_msgs + tf2_ros + visualization_msgs +) + +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}_local_planner.cpp + src/controller.cpp + src/calculations.cpp + src/details/derivative.cpp + src/details/integral.cpp + src/details/second_order_lowpass.cpp + src/visualization.cpp +) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) +include_directories(include) + +# Configure roslint for nodes + +# Roslint cpp +set(ROSLINT_CPP_OPTS "--filter=-legal/copyright,-whitespace/braces") +roslint_cpp() +roslint_add_test() + +install( + TARGETS + ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +# Install files +install( + FILES + README.md + ${PROJECT_NAME}_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install directories +install( + DIRECTORY + doc + launch + param + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) + catkin_add_gtest(unittests + test/unittests/test_calculations.cpp + test/unittests/test_derivative.cpp + test/unittests/test_fifo_array.cpp + test/unittests/test_integral.cpp + test/unittests/test_main.cpp + test/unittests/test_second_order_lowpass.cpp + ) + target_link_libraries(unittests ${catkin_LIBRARIES} ${PROJECT_NAME}) +endif() diff --git a/LICENSE b/LICENSE index f288702d..03c0b94e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,674 +1,202 @@ - 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 2023 Nobleo Technology B.V. + + 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 new file mode 100644 index 00000000..91c7e1ae --- /dev/null +++ b/README.md @@ -0,0 +1,220 @@ +# path_tracking_pid + +## Overview + +Path Tracking PID offers a tuneable PID control loop decouling steerting and forward velocity. The forward velocity is generated in an open loop fashion by using target velocities and accelerations. + +One of the tracking options uses a carrot of length l in front of the robot to determine the steering action based on the lateral error between the current Global Point (GP) and the Control point (CP): + +![Tracking carrot](doc/figures/tracking.png) + +If a smooth path is provided, the controller has the option to track the path with the base_link directly instead of lagging behind a carrot. In this case a Projected Global Point (PGP) is computed as well which is tracked by the CP. In this mode, the yaw error can also be used as control input. + +![Tracking Base Link](doc/figures/tracking_base_link.png) + + +The PID contains two closed loops: Lateral and angular loops, and an open loop: Longitudinal. For non-holonomic robots, the lateral and angular loops can be combine to compute steering actions. + +## Model Predictive Control + +As an optional feature, we use a Model Predictive Control (MPC) concept to regulate the forward velocity. The user can specify a maximum desired error bound to track the path. Using the model of the robot plus the controller settings, a predicted path is calculated and if the error bound is violated, the forward velocity of the robot is decreased until the predicted path lies within acceptable bounds. + +![MPC](doc/figures/mpc.png) + +## Tricycle model + +The tricycle model is supported as well. + +![Tricycle Model](doc/figures/tricycle_model.png) + +It is assumed the front wheel is steered and driven. The steered wheel link (SL) can be asymmetrically placed with respect to the BL. Limitations of steering and actuation are also taken into account. +## Anti collision + +When used as a plugin, Path Tracking PID uses the costmap (and the robot footprint from the costmap) for anti-collision. No evasive maneuvers will be preformed (sticking to the path is key). However when obstacles are detected in close proximity to the robot or in the direct line of direction of the robot, the forward velocity will be decreased. This results in safer velocities around obstacles. If an obstacle cannot be avoided or is too close to the robot, a standstill will occur and the navigation is cancelled. + +The maximum velocity of the robot will be scale linear with the costmap values around or in path of the robot. One can use the inflation layers (or even social layers) to construct these gradients in the costmap. + +**Keywords:** tracking, pid, local_planner, trajectory, model predictive control + +### License + +TBD + +**Author: Cesar Lopez, cesar.lopez@nobleo.nl.** + +**Maintainer: Cesar Lopez, cesar.lopez@nobleo.nl.** + +**Affiliation: [Nobleo Projects](https://www.nobleo.nl)** + + +The path_tracking_pid package has been tested under [ROS] Melodic and Ubuntu 18.04. + +[![Build Status](https://bitbucket.org/nobleo/path_tracking_pid/branch/develop)](https://bitbucket.org/nobleo/path_tracking_pid/branch/develop) + + +## Installation + +### Building from Source + +#### Dependencies + +- [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), + +#### Building + +To build from source, clone the latest version from this repository into your catkin workspace and compile the package using + + cd catkin_workspace/src + git clone https://bitbucket.org/nobleo/path_tracking_pid.git + cd ../ + catkin_make + +### Unit Tests + +Run the unit tests with `catkin run_tests path_tracking_pid` + +## Usage + +The path_tracking_pid is a plugin for [move_base_flex](http://wiki.ros.org/move_base_flex). + +To run move_base_flex with path_tracking_pid plugin: + + roslaunch path_tracking_pid path_tracking_pid_mbf.launch + +## Static parameters + +* **`base_link_frame`** (string, default: `base_link`) Name of the base link frame. + +* **`holonomic_robot`** (bool, default: `false`) True for an holonomic robot. --> Unmaintained, expect bugs + +* **`estimate_pose_angle`** (bool, default: `false`) Whether to take the pose angles from the path directly or to estimate them form consecutive path poses. + +* **`use_tricycle_model`** (bool, default: `false`) True for using tricycle model instead of differential drive. + +* **`steered_wheel_frame`** (string, default: `steer`) Name of the steered wheel frame. + +* **`use_mpc`** (bool, default: `false`) True for using MPC to regulate x velocity. + +## Configuring in RQT + +Tracking_pid parameters are all available through (rqt_)dynamic_reconfigure. The main parameters are: + +* **`l`** (double, default: `0.5`) Following distance from robot's rotational point to trajectory. +* **`track_base_link`** (bool, default: `false`) Whether to track the path using the base_link instead of the control point ahead. A smooth path is needed. +* **`init_vel_method`** (enum, default: `1`) Method to choose initial velocity on new paths. +* **`init_vel_max_diff`** (double, default: `0.5`) How much velocity difference is acceptable upon starting a new path. If internal state and current odometry's velocity differ more than this, the path will be aborted. Set to -1 to ignore this check. This check is only active if 'init_vel_method'==InternalSetpoint. + +Proportional, Integral and Derivative actions for the two closed loops: Lateral and angular loops. + +* **`Kp_lat`** (double, default: `1.0`) Proportional action gain for lateral loop. +* **`Ki_lat`** (double, default: `0.0`) Integral action gain for lateral loop. +* **`Kd_lat`** (double, default: `0.3`) Derivative action gain for lateral loop. +* **`Kp_ang`** (double, default: `1.0`) Proportional action gain for angular loop. +* **`Ki_ang`** (double, default: `0.0`) Integral action gain for angular loop. +* **`Kd_ang`** (double, default: `0.3`) Derivative action gain for angular loop. + +Each loop can be enabled/disabled separetly. + +* **`feedback_lat`** (boolean, default: `true`) Enable feedback lateral loop. +* **`feedback_ang`** (boolean, default: `false`) Enable feedback angular loop. + +Moreover, feedforward using trajectory velocity can be enabled/disabled. + +* **`feedforward_lat`** (boolean, default: `true`) Enable velocity feedforward for lateral loop. +* **`feedforward_ang`** (boolean, default: `false`) Enable velocity feedforward for angular loop. + +Target velocities and accelerations for generating the open loop forward velocity: + +* **`target_x_vel`** (double, default: `2.0`) Nominal target forward velocity. +* **`target_end_x_vel`** (double, default: `0.0`) Target forward velocity at the end of the path. +* **`target_x_acc`** (double, default: `2.0`) Desired acceleration at the beginning of the path. +* **`target_x_decc`** (double, default: `2.0`) Desired decceleration at the end of the path. + +Constraints on the generated velocities: + +* **`abs_minimum_x_vel`** (double, default: `0.025`) Minimum velocity of the vehicle, used to reach the very end of the path. +* **`max_error_x_vel`** (double, default: `1.0`) Maximum allowed x velocity error. +* **`max_yaw_vel`** (double, default: `2.0`) Maximum allowed yaw velocity +* **`max_yaw_acc`** (double, default: `2.0`) Maximum allowed yaw acceleration +* **`min_turning_radius`** (double, default: `0.0`) Minimum turning radius of the vehicle. + +Constraints on the steered wheel for the tricycle model: + +* **`max_steering_angle`** (double, default: `3.1416`) Maximum steering angle for tricycle model. + +* **`max_steering_x_vel`** (double, default: `3.0`) Maximum steering x velocity for tricycle model. + +* **`max_steering_x_acc`** (double, default: `2.0`) Maximum steering x acceleration for tricycle model. + +* **`max_steering_yaw_vel`** (double, default: `0.5`) Maximum steering yaw velocity for tricycle model. + +* **`max_steering_yaw_acc`** (double, default: `0.5`) Maximum steering yaw acceleration for tricycle model. + +Anti-collision parameters: + +* **`collision_look_ahead_length_offset`** (double, default: `1.0`) Offset in length to project rectangle collision along path. +* **`collision_look_ahead_resolution`** (double, default: `1.0`) Spatial resolution to project rectangle collision along path. + +Debug topic enable: + + * **`controller_debug_enabled`** (boolean, default: `false`) Enable debug topic. + +Parameters to configure MPC behavior: + +* **`mpc_max_error_lat`** (double, default: `0.5`) MPC maximum allowed lateral error. +* **`mpc_min_x_vel`** (double, default: `0.5`) MPC minimum absolute forward velocity. +* **`mpc_simulation_sample_time`** (double, default: `0.05`) MPC simulation sample time MPC maximum allowed iterations forward in time. +* **`mpc_max_fwd_iterations`** (int, default: `200`) Prediction iterations. Total simulation time will be then mpc_max_fwd_iterations*mpc_simulation_sample_time. +* **`mpc_max_vel_optimization_iterations`** (int, default: `5`) MPC maximum allowed velocity optimization iterations. + +![RQT reconfigure Tracking PID](doc/figures/rqt_path_tracking_pid.png) + +## Launch files + +* [launch/path_tracking_pid_mbf.launch](launch/path_tracking_pid_mbf.launch): Runs move_base_flex with path_tracking_pid plugin + +* [test/test_path_tracking_pid.launch](test/test_path_tracking_pid.launch): If all is well, a red square moves along a toothed path that covers a square. The square should be followed by the base_link frame. + +## Plugin +### path_tracking_pid/TrackingPidLocalPlanner +For use in move_base_flex. See launch/path_tracking_pid_mbf.launch to see an example. + +#### Subscribed Topics + +* **`path`** ([nav_msgs/Path]) + The path to follow. + +* **`odom`** ([nav_msgs/Odometry]) + Robot odometry. + +* **`/tf`** ([tf2_msgs/TFMessage]) + The position of the robot wrt. the frame in which the map is received. + +* **`vel_max`** ([std_msgs/Float64]) + Dynamic maximum velocity. To be used for example when maximum power demands are reached. + Higher level software could also use the reconfigure interface for this and set new speed values, but when required in a feedback-loop, this streaming topic is preferred. + +#### Published Topics + +* **`cmd_vel`** ([geometry_msgs/Twist]) + The Twist the robot should realize to track the desired path. + +* **`feedback`** ([path_tracking_pid/PidFeedback]) + Estimated duration remaining and progress towards final goal. + +* **`visualization_marker`** ([visualization_msgs/Marker]) + A Marker indicating the current control goal of the towards. + +* **`collision_footprint_marker`** ([visualization_msgs/Marker]) + A Marker indicating the footprint(s) along the path that are used for ahead collision detection. + +* **`debug`** ([path_tracking_pid/PidDebug]) + Intermediate values of the PID controller. Topic is silent by default, can be enabled via dynamic_reconfigure. + + +## Bugs & Feature Requests + +Please report bugs and request features using the [Issue Tracker](https://bitbucket.org/nobleo/path_tracking_pid/). + +[ROS]: http://www.ros.org +[rviz]: http://wiki.ros.org/rviz diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg new file mode 100755 index 00000000..46607c36 --- /dev/null +++ b/cfg/Pid.cfg @@ -0,0 +1,69 @@ +#!/usr/bin/env python +PACKAGE = "path_tracking_pid" + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t +from math import sqrt + +gen = ParameterGenerator() + +gen.add("l", double_t, 0, "Distance between axle and control point", 0.5, -10, 10) +gen.add("target_x_vel", double_t, 0, "Target forward velocity", 2.0, -10, 10) +gen.add("target_end_x_vel", double_t, 0, "Target forward velocity", 0.0, -10, 10) +gen.add("target_x_acc", double_t, 0, "Target forward acceleration", 2.0, 1e-9, 10) +gen.add("target_x_decc", double_t, 0, "Target forward decceleration", 2.0, 1e-9, 10) +gen.add("abs_minimum_x_vel", double_t, 0, "Absolute minimum velocity for reaching target", 0.025, 0.0, 0.5) +gen.add("max_error_x_vel", double_t, 0, "Maximum allowed error in forward velocity", 1.0, 1e-9, 10) +gen.add("max_x_vel", double_t, 0, "Maximum x velocity, used for scaling purposes", 5.0, 0, 10) +gen.add("max_yaw_vel", double_t, 0, "Maximum yaw velocity", 2.0, 0, 10) +gen.add("max_yaw_acc", double_t, 0, "Maximum yaw acceleration", 2.0, 0, 10) +gen.add("min_turning_radius", double_t, 0, "Minimum turning radius", 0.0, 0.0, 100) +gen.add("track_base_link", bool_t, 0, "Should the controller track the path using the base_link frame?", False) + +gen.add("init_vel_method", int_t, 0, "Initial velocity method", 1, 0, 3, edit_method=gen.enum([ + gen.const("Zero", int_t, 0, "Always start from zero"), + gen.const("InternalSetpoint", int_t, 1, "Last internal setpoint is new initial setpoint"), + gen.const("Odom", int_t, 2, "Start from current odometry value") + ], "Initial velocity method enum")) +gen.add("init_vel_max_diff", double_t, 0, "Maximum vel-diff allowed when starting a path (-1 to ignore, only active upon 'init_vel_method'==InternalSetpoint)", 0.5, -1, 10) + +gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 10) +gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 2) +gen.add("Kd_lat", double_t, 0, "Kd Lateral", 0.3, 0, 10) + +gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10) +gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2) +gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10) + +gen.add("lowpass_cutoff", double_t, 0, "Lowpass cutoff (Hz), 0 disables the filter", 0, 0, 1000) +gen.add("lowpass_damping", double_t, 0, "Lowpass damping", sqrt(2), 0, 10) + +gen.add("feedback_lat", bool_t, 0, "Enable lateral feedback?", True) +gen.add("feedback_ang", bool_t, 0, "Enable angular feedback?", False) + +gen.add("feedforward_lat", bool_t, 0, "Enable lateral feedforward?", False) +gen.add("feedforward_ang", bool_t, 0, "Enable angular feedforward?", False) + +gen.add("controller_debug_enabled", bool_t, 0, "Debug controller intermediate gains", False) + +gen.add("use_mpc", bool_t, 0, "Limit forward velocity based on predictions of the lateral error", False) +grp_mpc = gen.add_group("mpc_group", type="hide") +grp_mpc.add("mpc_simulation_sample_time", double_t, 0, "MPC simulation sample time", 0.05, 1e-9, 10) +grp_mpc.add("mpc_max_error_lat", double_t, 0, "MPC maximum allowed lateral error", 0.5, 1e-9, 10) +grp_mpc.add("mpc_max_fwd_iterations", int_t, 0, "MPC maximum allowed iterations forward in time", 200, 0, 1000000) +grp_mpc.add("mpc_min_x_vel", double_t, 0, "MPC minimum absolute forward velocity", 0.5, 1e-9, 10) +grp_mpc.add("mpc_max_vel_optimization_iterations", int_t, 0, "MPC maximum allowed velocity optimization iterations", 5, 1, 1000) + +grp_tricycle = gen.add_group("Tricycle") +grp_tricycle.add("max_steering_angle", double_t, 0, "Maximum steering angle for tricycle model", 3.1416, 0, 3.1416) +grp_tricycle.add("max_steering_x_vel", double_t, 0, "Maximum steering x velocity for tricycle model", 3.0, 0, 10) +grp_tricycle.add("max_steering_x_acc", double_t, 0, "Maximum steering x acceleration for tricycle model", 2.0, 0, 10) +grp_tricycle.add("max_steering_yaw_vel", double_t, 0, "Maximum steering yaw velocity for tricycle model", 0.5, 0, 10) +grp_tricycle.add("max_steering_yaw_acc", double_t, 0, "Maximum steering yaw acceleration for tricycle model", 0.5, 0, 10) + +gen.add("anti_collision", bool_t, 0, "Stop on lethal obstacles", False) +collision_group = gen.add_group("collision_group", type="hide") +collision_group.add("obstacle_speed_reduction", bool_t, 0, "Slow down on near obstacles", True) +collision_group.add("collision_look_ahead_length_offset", double_t, 0, "Offset in length to project rectangle collision along path", 1.0, 0, 5) +collision_group.add("collision_look_ahead_resolution", double_t, 0, "Spatial resolution to project rectangle collision along path", 1.0, 1e-9, 10) + +exit(gen.generate(PACKAGE, "path_tracking_pid", "Pid")) diff --git a/doc/figures/controller.ipe b/doc/figures/controller.ipe new file mode 100644 index 00000000..138457cd --- /dev/null +++ b/doc/figures/controller.ipe @@ -0,0 +1,497 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + +e_\theta + +208 680 m +256 680 l + +v_x + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + + +208 680 m +256 680 l + +\omega +Controller + +8 0 0 8 360 680 e + ++ ++ + +8 0 0 8 360 680 e + ++ ++ +\dot{\tilde{y}} + +400 636 m +400 684 l +416 684 l +416 636 l +h + + +416 660 m +440 660 l + +twist + +224 624 m +224 684 l +208 684 l +208 624 l +h + + +176 680 m +208 680 l + +GP + +176 680 m +208 680 l + +CP + +216 684 m +216 708 l +360 708 l +360 688 l + + +216 636 m +216 612 l +360 612 l +360 632 l + + +8 0 0 8 360 680 e + ++ ++ + +368 680 m +396 680 l +396 664 l + + +368 628 m +396 628 l +396 648 l + + +368 680 m +400 680 l + + +256 704 m +256 656 l +320 656 l +320 704 l +h + +$\dot{x}$ generator + +340 732 m +416 732 l +416 680 l +436 680 l + +e_y +\dot{\tilde{\theta}} + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + +e_\theta + +208 680 m +256 680 l + + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + + +208 680 m +256 680 l + +\omega + +8 0 0 8 360 680 e + ++ ++ + +8 0 0 8 360 680 e + ++ ++ +\dot{\tilde{y}} + +436 468 m +436 536 l +452 536 l +452 468 l +h + + +416 660 m +440 660 l + +twist + +224 624 m +224 684 l +208 684 l +208 624 l +h + + +176 680 m +208 680 l + +GP + +176 680 m +208 680 l + +CP + +216 684 m +216 708 l +360 708 l +360 688 l + + +216 636 m +216 612 l +360 612 l +360 632 l + + +368 500 m +436 500 l + + +256 704 m +256 656 l +320 656 l +320 704 l +h + + +336 552 m +412 552 l +412 528 l +432 528 l + +e_y +\dot{\tilde{\theta}} +v_x +$\dot{x}$ generator + +368 448 m +416 448 l +416 476 l +436 476 l + +v_y +non-holonomic +holonomic + + diff --git a/doc/figures/controller.png b/doc/figures/controller.png new file mode 100644 index 00000000..43df506c Binary files /dev/null and b/doc/figures/controller.png differ diff --git a/doc/figures/flowchart.ipe b/doc/figures/flowchart.ipe new file mode 100644 index 00000000..d3f5c307 --- /dev/null +++ b/doc/figures/flowchart.ipe @@ -0,0 +1,338 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +Global Planner +Local Planner +controller + +48 672 +144 672 +144 640 +48 640 u + + +192 672 +288 672 +288 640 +192 640 u + + +320 672 +416 672 +416 640 +320 640 u + + +284 656 m +324 656 l + + +368.145 705.333 m +368.234 670.666 l + +snd test/perf logger + +256 592 +352 592 +352 560 +256 560 u + + +432.163 698.34 m +425.746 690.014 +377 698.167 c + + +316.473 656 m +353.224 663.063 +346.575 689.685 c + + +239.941 705.333 m +239.932 670.667 l + + +512 752 m +512 640 l +592 640 l +592 752 l +h + +Robot + +476 656 m +512 655.758 l + +topic: global\_trajectory +msg: tracking\_pid::trajectory +topic: local\_trajectory +msg: tracking\_pid::traj\_point +topic: move\_base/cmd\_vel +msg: geometry\_msgs::Twist +topic: odometry\_map +msg: nav\_msgs::Odometry +/tf (robot pose) + +461.091 735.831 m +369.472 735.831 +359.917 735.831 +359.182 735.586 +353.792 732.891 +352.077 727.011 +352.052 720.001 c + + + diff --git a/doc/figures/flowchart.png b/doc/figures/flowchart.png new file mode 100644 index 00000000..c7ec2a83 Binary files /dev/null and b/doc/figures/flowchart.png differ diff --git a/doc/figures/mpc.ipe b/doc/figures/mpc.ipe new file mode 100644 index 00000000..1e78da07 --- /dev/null +++ b/doc/figures/mpc.ipe @@ -0,0 +1,370 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +320 256 m +320 256 l +320 256 l + + +128 256 m +128 448 l + + +128 256 m +320 256 l + + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + +224 348 m +320 444 +382.995 388.391 c + + +232 340 m +328 436 +379.445 375.964 c + + +216 356 m +312 452 +390.687 399.041 c + + +224 348 m +352 476 +405.48 418.568 c + + +224 348 m +328 452 +393.646 395.491 c + + +342.698 432.308 m +346.309 417.976 l + +x +y + + +184 308 m +240 364 l + + +184 308 m +232 308 l + +Desired trajectory \\ +Error Bounds \\ +Predicted Path \\ +Predicted path with altered velocity by MPC + +380 348 m +348 348 l +348 348 l + + +380 332 m +348 332 l + + +380 320 m +348 320 l + + +380 304 m +348 304 l + +P + +340 356 m +340 276 l +516 276 l +516 356 l +h + + +184 308 m +184 256 l +184 256 l + + +184 308 m +128 308 l + + + diff --git a/doc/figures/mpc.png b/doc/figures/mpc.png new file mode 100644 index 00000000..5bc6cbcb Binary files /dev/null and b/doc/figures/mpc.png differ diff --git a/doc/figures/rqt_path_tracking_pid.png b/doc/figures/rqt_path_tracking_pid.png new file mode 100644 index 00000000..eaaaebad Binary files /dev/null and b/doc/figures/rqt_path_tracking_pid.png differ diff --git a/doc/figures/tracking.ipe b/doc/figures/tracking.ipe new file mode 100644 index 00000000..ed2bb95e --- /dev/null +++ b/doc/figures/tracking.ipe @@ -0,0 +1,339 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +1.5788 0 0 1.5788 217.31 507.975 e + + +104 536 m +208 600 +320 540 +354.398 522.111 +399.131 505.566 +446.928 544.171 c + +e_y + +3.43377 0 0 3.43377 321.717 544.259 e + +CP +GP + +320 256 m +320 256 l +320 256 l + + +128 256 m +128 448 l + + +128 256 m +320 256 l + +x +y + +256 528 m +278.584 530.034 l + + +256.154 530.461 m +267.689 566.776 l + + +268.117 566.348 m +272.175 529.82 l + + +271.75 533.428 m +267.365 532.958 l +267.835 529.278 l + + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +232 676 m +292 736 l + + +171.483 519.383 m +165.505 562.989 l + +l +BL + + diff --git a/doc/figures/tracking.png b/doc/figures/tracking.png new file mode 100644 index 00000000..83510038 Binary files /dev/null and b/doc/figures/tracking.png differ diff --git a/doc/figures/tracking_base_link.ipe b/doc/figures/tracking_base_link.ipe new file mode 100644 index 00000000..a1197d38 --- /dev/null +++ b/doc/figures/tracking_base_link.ipe @@ -0,0 +1,398 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +1.5788 0 0 1.5788 217.31 507.975 e + + +104 536 m +208 600 +320 540 +354.398 522.111 +399.131 505.566 +446.928 544.171 c + +e_y +PGP + +320 256 m +320 256 l +320 256 l + + +128 256 m +128 448 l + + +128 256 m +320 256 l + +x +y +l + +127.401 549.628 m +163.302 564.548 +209.927 583.664 +259.581 598.241 +278.064 578.107 +278.724 560.944 +318.096 541.702 +352.131 523.518 +381.971 508.598 +409.013 506.733 +422.535 521.187 +429.994 528.647 +446.313 543.567 +461.233 555.223 c + + +3.43377 0 0 3.43377 234.921 624.227 e + +l + +176.458 569.069 m +250.756 592.027 l + + +167.817 566.773 m +147.35 615.654 l + +CP + +253.32 593.723 m +234.713 624.019 l + + +3.43377 0 0 3.43377 321.717 544.259 e + +GP +BL + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +171.483 519.383 m +165.505 562.989 l + + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +171.337 574.494 m +234.092 624.434 l + + +171.483 519.383 m +165.505 562.989 l + + +235.956 625.055 m +255.017 639.765 l + + +236.785 624.227 m +223.319 641.215 l + + +237.368 623.557 m +261.174 631.179 l + +e_\theta + +244.229 631.232 m +3.49671 0 0 -3.49671 243.901 627.751 247.257 626.77 a + + + diff --git a/doc/figures/tracking_base_link.png b/doc/figures/tracking_base_link.png new file mode 100644 index 00000000..9fb3df33 Binary files /dev/null and b/doc/figures/tracking_base_link.png differ diff --git a/doc/figures/tricycle_model.ipe b/doc/figures/tricycle_model.ipe new file mode 100644 index 00000000..2484669a --- /dev/null +++ b/doc/figures/tricycle_model.ipe @@ -0,0 +1,356 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +160 496 m +160 592 l +352 592 l +352 496 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +256.212 460.259 m +256.006 524.517 l + + +172 544 m +172 584 l + +BL + +260 572 m +260 612 l +252 612 l +252 572 l +h + + +256 592 m +256 624 l + + +256 592 m +232 592 l + + +236 608 m +236 636 l + + +236 608 m +256 608 l +256 528 l + + +236 608 m +256 608 l +256 528 l + +SL + + +236.319 608.651 m +246.496 612.3 +251.437 612.459 +257.493 617.24 +261.955 608.953 c + +(x_w,y_w) + +226.257 618.674 m +13.8649 0 0 -13.8649 235.819 608.634 235.978 622.498 a + + +231.994 623.136 m +233.11 626.164 +239.484 627.28 +254.943 629.511 c + +\delta_w + +256 460 m +236 608 l + + +246.736 527.461 m +67.8648 0 0 -67.8648 256.13 460.249 256.056 528.114 a + +\theta_w + +251.63 529.708 m +252.319 533.706 +258.523 534.533 +264.037 535.498 +266.656 543.769 +273.136 545.561 c + + + diff --git a/doc/figures/tricycle_model.png b/doc/figures/tricycle_model.png new file mode 100644 index 00000000..6aa055f7 Binary files /dev/null and b/doc/figures/tricycle_model.png differ diff --git a/doc/integral_tustin.ipynb b/doc/integral_tustin.ipynb new file mode 100644 index 00000000..27538c95 --- /dev/null +++ b/doc/integral_tustin.ipynb @@ -0,0 +1,130 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# How to discretize a integral filter with Tustin's method\n", + "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from sympy import *" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "s, T, z = symbols('s,T,z')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "First our continous time system" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "sys = 1 / s" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate to discrete" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{T \\left(z + 1\\right)}{2 \\left(z - 1\\right)}$" + ], + "text/plain": [ + "T*(z + 1)/(2*(z - 1))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n", + "sys" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate that to C++" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "```\n", + "H = y/u = T / 2 * (z + 1)/(z - 1)\n", + "\n", + "u * T / 2 * (z + 1) = y * (z - 1)\n", + "u * T / 2 * (1 + z^-1) = y * (1 - z^-1)\n", + "T / 2 * (u[0] + u[1]) = y[0] - y[1]\n", + "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n", + "```\n", + "\n", + "```c++\n", + "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n", + "```" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/doc/second_order_lowpass_tustin.ipynb b/doc/second_order_lowpass_tustin.ipynb new file mode 100644 index 00000000..9ef5eca0 --- /dev/null +++ b/doc/second_order_lowpass_tustin.ipynb @@ -0,0 +1,275 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# How to discretize a second order lowpass with Tustin's method\n", + "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from sympy import *" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "a, s, d, T, z = symbols('a,s,d,T,z')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "First our continous time system" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# a = 2*pi*c\n", + "sys = 1 / (1/a**2 * s**2 + 2*d/a * s + 1)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate to discrete" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{1}{1 + \\frac{4 d \\left(z - 1\\right)}{T a \\left(z + 1\\right)} + \\frac{4 \\left(z - 1\\right)^{2}}{T^{2} a^{2} \\left(z + 1\\right)^{2}}}$" + ], + "text/plain": [ + "1/(1 + 4*d*(z - 1)/(T*a*(z + 1)) + 4*(z - 1)**2/(T**2*a**2*(z + 1)**2))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n", + "sys" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2}}{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2} + 4 T a d z^{2} - 4 T a d + 4 z^{2} - 8 z + 4}$" + ], + "text/plain": [ + "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "print(cancel(sys))\n", + "cancel(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[T**2*a**2, 2*T**2*a**2, T**2*a**2]" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "num = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2, z)\n", + "num.all_coeffs()" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[T**2*a**2 + 4*T*a*d + 4, 2*T**2*a**2 - 8, T**2*a**2 - 4*T*a*d + 4]" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "den = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4, z)\n", + "den.all_coeffs()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Try to simplify" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{b^{2} z^{2} + 2 b^{2} z + b^{2}}{b^{2} z^{2} + 2 b^{2} z + b^{2} + 4 b d z^{2} - 4 b d + 4 z^{2} - 8 z + 4}$" + ], + "text/plain": [ + "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "b = symbols('b')\n", + "# a*T = b -> T = b/a\n", + "sys = sys.subs(T, b/a)\n", + "print(cancel(sys))\n", + "cancel(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[b**2, 2*b**2, b**2]" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "num = Poly(b**2*z**2 + 2*b**2*z + b**2, z)\n", + "num.all_coeffs()" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[b**2 + 4*b*d + 4, 2*b**2 - 8, b**2 - 4*b*d + 4]" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "den = Poly(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4, z)\n", + "den.all_coeffs()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate that to C++" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "```c++\n", + "auto a = 2 * M_PI * c;\n", + "auto b = T * a;\n", + "y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] -\n", + " (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) /\n", + " (pow(b, 2) + 4 * T * a * d + 4);\n", + "```" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp new file mode 100644 index 00000000..b665983b --- /dev/null +++ b/include/path_tracking_pid/controller.hpp @@ -0,0 +1,256 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace path_tracking_pid +{ +struct TricycleSteeringCmdVel +{ + double steering_angle = 0.0; + double steering_angle_velocity = 0.0; + double speed = 0.0; + double acceleration = 0.0; +}; + +class Controller : private boost::noncopyable +{ +public: + /** + * Set holonomic configuration of the controller + * @param holonomic is holonomic robot? + */ + void setHolonomic(bool holonomic); + + /** + * Enable estimation of pose angles by looking at consecutive path points + * @param estimate_pose_angle + */ + void setEstimatePoseAngle(bool estimate_pose_angle); + + /** + * Set static configuration of the controller + * @param tricycle_model_enabled If tricycle model should be used + * @param estimate_pose_angle The transformation from base to steered wheel + */ + void setTricycleModel( + bool tricycle_model_enabled, const tf2::Transform & tf_base_to_steered_wheel); + + /** + * Set plan + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param global_plan Plan to follow + * @return whether the plan was successfully updated or not + */ + bool setPlan( + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const std::vector & global_plan); + + /** + * Set plan + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param tf_base_to_steered_wheel Where is the steered wheel now? + * @param steering_odom_twist Steered wheel odometry + * @param global_plan Plan to follow + * @return whether the plan was successfully updated or not + */ + bool setPlan( + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const tf2::Transform & tf_base_to_steered_wheel, + const geometry_msgs::Twist & steering_odom_twist, + const std::vector & global_plan); + + /** Result of findPoseOnPlan(). */ + struct FindPoseOnPlanResult + { + tf2::Transform pose; + std::size_t path_pose_idx = 0; + double distance_to_goal = 0; + std::size_t last_visited_pose_index = 0; + }; + + /** + * Find pose on plan by looking at the surroundings of last known pose. + * @param[in] current_tf Where is the robot now? + * @param[in,out] global_plan_index Global plan index where the search should start. Updated to + * current global plan index once found. + * @return Found pose on plan and related data. + */ + FindPoseOnPlanResult findPoseOnPlan( + const tf2::Transform & current_tf, std::size_t & global_plan_index) const; + + // Result of update() and update_with_limits(). + struct UpdateResult + { + geometry_msgs::Twist velocity_command; // Velocity command + double eda = 0; // Estimated duration of arrival + double progress = 0; // Progress along the path [0,1] + PidDebug pid_debug; // Information to debug the controller + }; + + /** + * Run one iteration of a PID controller + * @param target_x_vel robot will try to reach target x velocity within (de)acceleration limits + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param dt Duration since last update + * @return Update result + */ + UpdateResult update( + double target_x_vel, double target_end_x_vel, const tf2::Transform & current_tf, + const geometry_msgs::Twist & odom_twist, ros::Duration dt); + + /** + * Run one iteration of a PID controller with velocity limits applied + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param dt Duration since last update + * @return Update result + */ + UpdateResult update_with_limits( + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt); + + /** + * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds + * @param current_tf Where is the robot now? + * @param odom_twist Robot odometry + * @return Velocity command + */ + double mpc_based_max_vel( + double target_x_vel, const tf2::Transform & current_tf, + const geometry_msgs::Twist & odom_twist); + + /** + * Set dynamic parameters for the PID controller + * @param config + */ + void configure(path_tracking_pid::PidConfig & config); + + /** + * Set whether the controller is enabled + * @param value enable controller? + */ + void setEnabled(bool value); + + /** + * Reset controller state + */ + void reset(); + + /** + * Gets current dynamic configuration of the controller + * @return current controller configuration + */ + path_tracking_pid::PidConfig getConfig(); + + // Inline get-functions for transforms + tf2::Transform getCurrentGoal() const { return current_goal_; } + tf2::Transform getCurrentWithCarrot() const { return current_with_carrot_; } + tf2::Transform getCurrentPosOnPlan() const { return current_pos_on_plan_; } + + /** Get current forward velocity. */ + double getCurrentForwardVelocity() const { return controller_state_.current_x_vel; } + + /** Get current yaw velocity. */ + double getCurrentYawVelocity() const { return controller_state_.current_yaw_vel; } + + /** Indicates if the end is reached. */ + bool isEndReached() const { return controller_state_.end_reached; } + + /** Gets the current global plan index. */ + std::size_t getCurrentGlobalPlanIndex() const + { + return controller_state_.current_global_plan_index; + } + + // Set new vel_max_external value + void setVelMaxExternal(double value); + + // Set new vel_max_obstacle value + void setVelMaxObstacle(double value); + + // Get vel_max_obstacle value + double getVelMaxObstacle() const; + +private: + struct ControllerState + { + size_t current_global_plan_index = 0; + double current_x_vel = 0.0; + double current_yaw_vel = 0.0; + double previous_steering_angle = 0.0; + double previous_steering_x_vel = 0.0; + double previous_steering_yaw_vel = 0.0; + bool end_phase_enabled = false; + bool end_reached = false; + double tracking_error_lat = 0.0; + double tracking_error_ang = 0.0; + // Errors with little history + details::SecondOrderLowpass error_lat; + details::SecondOrderLowpass error_ang; + details::Integral error_integral_lat; + details::Integral error_integral_ang; + details::Derivative error_deriv_lat; + details::Derivative error_deriv_ang; + }; + + geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); + TricycleSteeringCmdVel computeTricycleModelInverseKinematics( + const geometry_msgs::Twist & cmd_vel); + /** + * Output some debug information about the current parameters + */ + void printParameters() const; + + path_tracking_pid::PidConfig config_; + ControllerState controller_state_; + + // Global Plan variables + std::vector global_plan_tf_; // Global plan vector + std::vector distance_to_goal_vector_; // Vector with distances to goal + std::vector turning_radius_inv_vector_; // Vector with computed turning radius inverse + tf2::Transform current_goal_; + tf2::Transform current_pos_on_plan_; + tf2::Transform current_with_carrot_; + + // Auxiliary variables + double current_target_x_vel_ = 0.0; + double control_effort_long_ = 0.0; // output of pid controller + double control_effort_lat_ = 0.0; // output of pid controller + double control_effort_ang_ = 0.0; // output of pid controller + + bool enabled_ = true; + bool holonomic_robot_enable_ = false; + bool estimate_pose_angle_ = false; + + // feedforward controller + double feedforward_lat_ = 0.0; + double feedforward_ang_ = 0.0; + + // tricycle model + bool use_tricycle_model_ = false; + tf2::Transform tf_base_to_steered_wheel_; + std::array, 2> inverse_kinematics_matrix_{}; + std::array, 2> forward_kinematics_matrix_{}; + + // Velocity limits that can be active external to the pid controller: + double vel_max_external_ = + INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) + double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected +}; + +} // namespace path_tracking_pid diff --git a/include/path_tracking_pid/details/derivative.hpp b/include/path_tracking_pid/details/derivative.hpp new file mode 100644 index 00000000..a4df7e07 --- /dev/null +++ b/include/path_tracking_pid/details/derivative.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time derivative filter + */ +class Derivative +{ +public: + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Derivative of the signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/details/fifo_array.hpp b/include/path_tracking_pid/details/fifo_array.hpp new file mode 100644 index 00000000..965499e0 --- /dev/null +++ b/include/path_tracking_pid/details/fifo_array.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include + +namespace path_tracking_pid::details +{ +// Fixed-size array-like FIFO buffer. All elements are value initialized upon construction. +template +class FifoArray +{ +public: + // Pushes the given value to the front of the buffer (index = 0). All elements already in the buffer are moved + // towards the end of the buffer (index = size - 1). The last element is removed from the buffer. + constexpr void push(const value_type & value) + { + std::copy_backward(data_.begin(), std::prev(data_.end()), data_.end()); + data_[0] = value; + } + + // Value initializes all elements in the buffer. + constexpr void reset() { data_ = {}; } + + // Read-only access to the element at the given index. + constexpr const value_type & operator[](std::size_t index) const { return data_[index]; } + + // Read-write access to the element at the given index. + value_type & operator[](std::size_t index) { return data_[index]; } + + // Read-only access to the element at the given index (with compile-time range check). + template + constexpr const value_type & at() const + { + static_assert(index < size); + return data_[index]; + } + +private: + std::array data_{}; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/details/integral.hpp b/include/path_tracking_pid/details/integral.hpp new file mode 100644 index 00000000..cc0e6bf4 --- /dev/null +++ b/include/path_tracking_pid/details/integral.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time integral filter + */ +class Integral +{ + constexpr static auto NaN = std::numeric_limits::quiet_NaN(); + +public: + Integral() = default; + + /** + * @brief Construct an integral filter + * @param windup_limit Integral windup limit + */ + explicit Integral(double windup_limit); + + /** + * @brief Change the parameters of the filter + * @param windup_limit Integral windup limit + */ + void configure(double windup_limit); + + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Integral of the signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; + FifoArray y_ = {}; + double windup_limit_ = NaN; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/details/second_order_lowpass.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp new file mode 100644 index 00000000..bbf873f5 --- /dev/null +++ b/include/path_tracking_pid/details/second_order_lowpass.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time second order lowpass filter + */ +class SecondOrderLowpass +{ + constexpr static auto NaN = std::numeric_limits::quiet_NaN(); + +public: + /** + * @brief Construct a SecondOrderLowpass instance with NaNs + */ + SecondOrderLowpass() = default; + + /** + * @brief Construct a SecondOrderLowpass instance + * @param cutoff frequency in Hz, 0 disables the filter + * @param damping frequency in Hz + */ + SecondOrderLowpass(double cutoff, double damping); + + /** + * @brief Change the parameters of the filter + * @param cutoff frequency in Hz, 0 disables the filter + * @param damping frequency in Hz + */ + void configure(double cutoff, double damping); + + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Lowpass-filtered signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; + FifoArray y_ = {}; + double cutoff_ = NaN; + double damping_ = NaN; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp new file mode 100644 index 00000000..ef4087c6 --- /dev/null +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -0,0 +1,192 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) + +namespace path_tracking_pid +{ +class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, + private boost::noncopyable +{ +private: + using polygon_t = boost::geometry::model::ring; + + static inline polygon_t union_(const polygon_t & polygon1, const polygon_t & polygon2) + { + std::vector output_vec; + boost::geometry::union_(polygon1, polygon2, output_vec); + return output_vec.at(0); // Only first vector element is filled + } + +public: + /** + * @brief Initialize local planner + * @param name The name of the planner + * @param tf a pointer to TransformListener in TF Buffer + * @param costmap Costmap indicating free/occupied space + */ + void initialize( + std::string name, tf2_ros::Buffer * tf, costmap_2d::Costmap2DROS * costmap) override; + + /** + * @brief Set the plan we should be following + * @param global_plan Plan to follow as closely as we can + * @return whether the plan was successfully updated or not + */ + bool setPlan(const std::vector & global_plan) override; + + /** + * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity + * and message are not set. See the interface in move base flex. + * @param pose Current robot pose + * @param velocity + * @param cmd_vel Output the velocity command + * @param message + * @return a status code defined in the move base flex ExePath action. + */ + uint32_t computeVelocityCommands( + const geometry_msgs::PoseStamped & pose, const geometry_msgs::TwistStamped & velocity, + geometry_msgs::TwistStamped & cmd_vel, std::string & message) override; + + /** + * @brief Returns true, if the goal is reached. Currently does not respect the parameters given. + * @param dist_tolerance Tolerance in distance to the goal + * @param angle_tolerance Tolerance in the orientation to the goals orientation + * @return true, if the goal is reached + */ + bool isGoalReached(double dist_tolerance, double angle_tolerance) override; + + /** + * @brief Canceles the planner. + * @return True on cancel success. + */ + bool cancel() override; + + /** Enumeration for custom SUCCESS feedback codes. See default ones: + * https://github.com/magazino/move_base_flex/blob/master/mbf_msgs/action/ExePath.action + */ + enum class ComputeVelocityCommandsResult { SUCCESS = 0, GRACEFULLY_CANCELLING = 1 }; + +private: + /** + * @brief Calculates the velocity command based on the current robot pose given by pose. + * @return Velocity command on success, empty optional otherwise. + */ + std::optional computeVelocityCommands(); + + /** + * @brief Returns true, if the goal is reached. + * @return true, if the goal is reached + */ + bool isGoalReached() const; + + /** + * Accept a new configuration for the PID controller + * @param config the new configuration + */ + void reconfigure_pid(path_tracking_pid::PidConfig & config); + + void pauseCallback(std_msgs::Bool pause); + + void curOdomCallback(const nav_msgs::Odometry & odom_msg); + + void velMaxExternalCallback(const std_msgs::Float64 & msg); + + /** + * @brief Project an amount of steps in the direction of movement based on velocity + * @return Projected steps + */ + std::vector projectionSteps(); + + /** + * @brief Expand the footprint with the projected steps + * @param footprint + * @param projected_steps + * @param viz Used for marker publishing + * @param viz_frame Used for marker publishing + * @return Projected footprint + */ + static polygon_t projectionFootprint( + const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame); + + /** + * @brief Projects the footprint along the projected steps and determines maximum cost in that area + * @param costmap2d + * @param footprint + * @param projected_steps + * @param viz Used for marker publishing + * @param viz_frame Used for marker publishing + * @return Maximum cost + */ + static uint8_t projectedCollisionCost( + costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame); + + nav_msgs::Odometry latest_odom_; + ros::Time prev_time_; + ros::Duration prev_dt_; + path_tracking_pid::Controller pid_controller_; + + // Obstacle collision detection + costmap_2d::Costmap2DROS * costmap_ = nullptr; + + // Cancel flags (multi threaded, so atomic bools) + std::atomic active_goal_{false}; + std::atomic cancel_requested_{false}; + std::atomic cancel_in_progress_{false}; + + // dynamic reconfiguration + boost::recursive_mutex config_mutex_; + std::unique_ptr> pid_server_; + + tf2_ros::Buffer * tf_ = nullptr; + geometry_msgs::TransformStamped tfCurPoseStamped_; + + ros::Publisher debug_pub_; // Debugging of controller internal parameters + + // Rviz visualization + std::unique_ptr visualization_; + ros::Publisher path_pub_; + + ros::Subscriber sub_odom_; + ros::Publisher feedback_pub_; + + ros::Subscriber sub_vel_max_external_; + + std::string map_frame_; + std::string base_link_frame_; + bool initialized_ = false; + + // Used for tricycle model + bool use_tricycle_model_ = false; + std::string steered_wheel_frame_; + geometry_msgs::TransformStamped tf_base_to_steered_wheel_stamped_; + + // Controller logic + bool controller_debug_enabled_ = false; +}; + +} // namespace path_tracking_pid diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp new file mode 100644 index 00000000..bc06d764 --- /dev/null +++ b/include/path_tracking_pid/visualization.hpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include + +#include +#include + +namespace path_tracking_pid +{ +class Visualization +{ +public: + explicit Visualization(ros::NodeHandle nh); + + void publishControlPoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishCollisionObject( + const std_msgs::Header & header, uint8_t cost, const tf2::Vector3 & point); + void publishExtrapolatedPoses( + const std_msgs::Header & header, const std::vector & steps); + void publishgGoalPosesOnPath( + const std_msgs::Header & header, const std::vector & path); + void publishCollisionFootprint( + const std_msgs::Header & header, const std::vector & footprint); + void publishCollisionPolygon( + const std_msgs::Header & header, const std::vector & hull); + +private: + ros::Publisher marker_pub_; + + void publishSphere( + const std_msgs::Header & header, const std::string & ns, const tf2::Transform & pose, + const std_msgs::ColorRGBA & color); + void publishSphere( + const std_msgs::Header & header, const std::string & ns, const geometry_msgs::Pose & pose, + const std_msgs::ColorRGBA & color); +}; + +} // namespace path_tracking_pid diff --git a/launch/path_tracking_pid_mbf.launch b/launch/path_tracking_pid_mbf.launch new file mode 100644 index 00000000..a3f7546b --- /dev/null +++ b/launch/path_tracking_pid_mbf.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/maps/grid.png b/maps/grid.png new file mode 100644 index 00000000..100ca0c5 Binary files /dev/null and b/maps/grid.png differ diff --git a/maps/grid.yaml b/maps/grid.yaml new file mode 100644 index 00000000..f97769b0 --- /dev/null +++ b/maps/grid.yaml @@ -0,0 +1,6 @@ +image: grid.png +resolution: 0.05 +origin: [-5, -5, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/msg/PidDebug.msg b/msg/PidDebug.msg new file mode 100644 index 00000000..182c4f81 --- /dev/null +++ b/msg/PidDebug.msg @@ -0,0 +1,12 @@ +# Error topic containing the 'control' error on which the PID acts +geometry_msgs/Twist control_error +# Error topic containing the 'tracking' error, i.e. the real error between path and tracked link +geometry_msgs/Twist tracking_error +# Control values +geometry_msgs/Twist proportional +geometry_msgs/Twist integral +geometry_msgs/Twist derivative +geometry_msgs/Twist feedforward +float32 steering_angle +float32 steering_yaw_vel +float32 steering_x_vel diff --git a/msg/PidFeedback.msg b/msg/PidFeedback.msg new file mode 100644 index 00000000..30a5010a --- /dev/null +++ b/msg/PidFeedback.msg @@ -0,0 +1,2 @@ +duration eda # Estimated (optimistic) duration remaining +float32 progress # Progress in distance of the path traveled diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..553668e9 --- /dev/null +++ b/package.xml @@ -0,0 +1,41 @@ + + + path_tracking_pid + 2.19.0 + Follows a trajectory with open-loop speed and closed loop (pid) lateral control + + Cesar Lopez + + Cesar Lopez + Michiel Francke + + Apache-2.0 + + catkin + message_generation + roslint + rostest + message_runtime + actionlib + actionlib_msgs + dynamic_reconfigure + geometry_msgs + mbf_costmap_core + mbf_msgs + nav_msgs + pluginlib + roscpp + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + mbf_costmap_nav + message_runtime + mobile_robot_simulator + rospy + + + + + diff --git a/param/controllers.yaml b/param/controllers.yaml new file mode 100644 index 00000000..3fe1be86 --- /dev/null +++ b/param/controllers.yaml @@ -0,0 +1,19 @@ +map_frame: 'map' +global_frame: 'map' +controller_frequency: 20.0 +controller_max_retries: 0 +controllers: + - name: 'PathTrackingPID' + type: 'path_tracking_pid/TrackingPidLocalPlanner' +PathTrackingPID: + holonomic_robot: false + track_base_link: true + estimate_pose_angle: false + base_link_frame: 'base_link' + target_x_acc: 1.0 + target_x_decc: 0.3 + abs_minimum_x_vel: 0.0 + anti_collision: true + use_mpc: false + feedforward_ang: true + controller_debug_enabled: true diff --git a/param/path_tracking_pid.yaml b/param/path_tracking_pid.yaml new file mode 100644 index 00000000..d6ee6669 --- /dev/null +++ b/param/path_tracking_pid.yaml @@ -0,0 +1,38 @@ +holonomic_robot: false +track_base_link: true +estimate_pose_angle: false +base_link_frame: 'base_link' +Kd_ang: 0.3 +Kd_lat: 0.3 +Ki_ang: 0.0 +Ki_lat: 0.0 +Kp_ang: 1.0 +Kp_lat: 1.0 +l: 2.0 +feedback_ang: false +feedback_lat: true +feedforward_ang: false +feedforward_lat: true +controller_debug_enabled: true +target_end_x_vel: 0.0 +target_x_acc: 2.0 +target_x_decc: 2.0 +target_x_vel: 2.0 +abs_minimum_x_vel: 0.025 +max_error_x_vel: 1.0 +max_yaw_vel: 2.0 +max_yaw_acc: 2.0 +min_turning_radius: 0.0 + +# MPC +use_mpc: false +mpc_max_error_lat: 0.5 +mpc_max_fwd_iter: 200 +mpc_max_vel_optimization_iterations: 5 +mpc_min_x_vel: 0.5 +mpc_simulation_sample_time: 0.05 + +# anti-collision: +anti_collision: true +obstacle_speed_reduction: true +collision_look_ahead_resolution: 1.0 # [m] diff --git a/path_tracking_pid_plugin.xml b/path_tracking_pid_plugin.xml new file mode 100644 index 00000000..4c6e7031 --- /dev/null +++ b/path_tracking_pid_plugin.xml @@ -0,0 +1,7 @@ + + + + Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity + + + diff --git a/src/calculations.cpp b/src/calculations.cpp new file mode 100644 index 00000000..991f1d89 --- /dev/null +++ b/src/calculations.cpp @@ -0,0 +1,122 @@ +#include "calculations.hpp" + +#include + +#include +#include +#include +#include + +namespace path_tracking_pid +{ +std::vector deltas_of_plan(const std::vector & plan) +{ + auto result = std::vector{}; + + if (plan.size() < 2) { + return result; + } + + result.reserve(plan.size() - 1); + std::transform( + plan.cbegin(), plan.cend() - 1, plan.cbegin() + 1, std::back_inserter(result), + [](const tf2::Transform & a, const tf2::Transform & b) { return a.inverseTimes(b); }); + + return result; +} + +std::vector distances_to_goal(const std::vector & deltas) +{ + auto result = std::vector{}; + + result.reserve(deltas.size() + 1); + std::transform( + deltas.cbegin(), deltas.cend(), std::back_inserter(result), [](const tf2::Transform & d) { + const auto & origin = d.getOrigin(); + return std::hypot(origin.x(), origin.y()); + }); + result.push_back(0.0); + + std::partial_sum(result.crbegin(), result.crend(), result.rbegin()); + + return result; +} + +std::vector inverse_turning_radiuses(const std::vector & deltas) +{ + auto result = std::vector{}; + + result.reserve(deltas.size() + 1); + std::transform( + deltas.cbegin(), deltas.cend(), std::back_inserter(result), [](const tf2::Transform & d) { + const auto & origin = d.getOrigin(); + const auto dpX = origin.x(); + const auto dpY = origin.y(); + const auto dpXY2 = std::pow(dpX, 2) + std::pow(dpY, 2); + if (dpXY2 < FLT_EPSILON) { + return std::numeric_limits::infinity(); + } + return (2 * dpY) / dpXY2; + }); + result.push_back(0.0); + + return result; +} + +bool is_in_direction_of_target( + const tf2::Transform & current, const tf2::Vector3 & target, double velocity) +{ + const auto delta = target - current.getOrigin(); + const auto projection = current.getBasis().tdotx(delta); + + return !std::signbit(projection * velocity); +} + +double distSquared(const tf2::Transform & a, const tf2::Transform & b) +{ + return a.getOrigin().distance2(b.getOrigin()); +} + +tf2::Transform closestPoseOnSegment( + const tf2::Transform & point, const tf2::Transform & segment_start, + const tf2::Transform & segment_end, bool estimate_pose_angle) +{ + const double l2 = distSquared(segment_start, segment_end); + if (l2 == 0) { + return segment_end; + } + + tf2::Transform result; + + const double t = std::clamp( + ((point.getOrigin().x() - segment_start.getOrigin().x()) * + (segment_end.getOrigin().x() - segment_start.getOrigin().x()) + + (point.getOrigin().y() - segment_start.getOrigin().y()) * + (segment_end.getOrigin().y() - segment_start.getOrigin().y())) / + l2, + 0.0, 1.0); + result.setOrigin(tf2::Vector3( + segment_start.getOrigin().x() + + t * (segment_end.getOrigin().x() - segment_start.getOrigin().x()), + segment_start.getOrigin().y() + + t * (segment_end.getOrigin().y() - segment_start.getOrigin().y()), + 0.0)); + + const auto yaw = estimate_pose_angle + ? atan2( + segment_end.getOrigin().y() - segment_start.getOrigin().y(), + segment_end.getOrigin().x() - segment_start.getOrigin().x()) + : tf2::getYaw(segment_start.getRotation()); + tf2::Quaternion pose_quaternion; + pose_quaternion.setRPY(0.0, 0.0, yaw); + result.setRotation(pose_quaternion); + + return result; +} + +tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance) +{ + return tf2::Transform{pose.getBasis(), pose * tf2::Vector3{control_distance, 0, 0}}; +} + +} // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp new file mode 100644 index 00000000..29235402 --- /dev/null +++ b/src/calculations.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include + +#include + +namespace path_tracking_pid +{ +/** + * Determine the deltas between consecutive poses in the given plan. Each delta is the inverse of a + * pose times the next pose. If the plan contains fewer than 2 poses, the output is empty. + * + * @param[in] plan Plan to process. + * @return Deltas between consecutive poses. + */ +std::vector deltas_of_plan(const std::vector & plan); + +/** + * Determine the distances to the goal for each pose of a plan, based on the given deltas of that + * plan. The 2D distance is calculated; the z component is ignored. + * + * @param[in] deltas Deltas between consecutive poses of a plan. (Result of deltas_of_plan().) + * @return Distances to the goal. + */ +std::vector distances_to_goal(const std::vector & deltas); + +/** + * Determine the inverse turning radiuses for each pose of a plan, based on the given deltas of that + * plan. The 2D radius is calculated; the z component is ignored. + * + * @param[in] deltas Deltas between consecutive poses of a plan. (Result of deltas_of_plan().) + * @return Inverse turning radiuses. + */ +std::vector inverse_turning_radiuses(const std::vector & deltas); + +/** + * Determine if the given current pose is in the direction of the given target position taking the + * given velocity into account. + * + * @param[in] current Current pose. + * @param[in] target Target position. + * @param[in] velocity Forward velocity in m/s. + * @return True if it is in the direction of the target, false otherwise. + */ +bool is_in_direction_of_target( + const tf2::Transform & current, const tf2::Vector3 & target, double velocity); + +/** + * Returns the square distance between two poses. + * + * @param[in] a Pose A. + * @param[in] b Pose B. + * @return Distance between A and B. + */ +double distSquared(const tf2::Transform & a, const tf2::Transform & b); + +/** + * @brief Closest pose between a line segment and a point + * + * Calculate the closest pose between the line segment bounded by `segment_start` and `segment_end` + * and point `point`. + * + * @param[in] point The point + * @param[in] segment_start Start of the line segment + * @param[in] segment_end End of the line segment + * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line + * segment (true) or if the pose angle from segment_start should be + * used. + * @return The pose projection of the closest point. + */ +tf2::Transform closestPoseOnSegment( + const tf2::Transform & point, const tf2::Transform & segment_start, + const tf2::Transform & segment_end, bool estimate_pose_angle); + +/** + * Determine the control point pose based on the given pose and control distance. + * + * @param[in] pose Pose to transform. + * @param[in] control_distance Control distance to use. + * @return Control point pose. + */ +tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance); + +} // namespace path_tracking_pid diff --git a/src/common.hpp b/src/common.hpp new file mode 100644 index 00000000..97eeade6 --- /dev/null +++ b/src/common.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include + +namespace path_tracking_pid +{ +inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity + +// Converts an enumeration to its underlying type. +template +constexpr std::underlying_type_t to_underlying(enum_type value) noexcept +{ + return static_cast>(value); +} + +/** + * Converts input (of input_type) to result_type. Like tf2::fromMsg() but using a return value + * instead of an output parameter. + * + * @tparam result_type Resulting type of the conversion. Should be a tf2 type. + * @tparam input_type Input type for the conversion. Should be a message type. + * @param[in] input Input object to convert. + * @return Converted object. + */ +template < + typename result_type, typename input_type, + typename = std::enable_if_t>> +result_type tf2_convert(const input_type & input) +{ + result_type result; + tf2::fromMsg(input, result); + return result; +} + +} // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp new file mode 100644 index 00000000..7f3bbbbb --- /dev/null +++ b/src/controller.cpp @@ -0,0 +1,974 @@ +// +// Created by nobleo on 11-9-18. +// + +#include +#include + +#include +#include +#include +#include + +#include "calculations.hpp" +#include "common.hpp" + +namespace path_tracking_pid +{ +namespace +{ +constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] +constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) + +// Upper and lower saturation limits +constexpr double lat_upper_limit = 100.0; +constexpr double lat_lower_limit = -100.0; + +constexpr double ang_upper_limit = 100.0; +constexpr double ang_lower_limit = -100.0; + +// Anti-windup term. Limits the absolute value of the integral term. +constexpr double windup_limit = 1000.0; + +// Indicates if the angle of the cur pose is obtuse (with respect to the prev and next poses). +bool is_pose_angle_obtuse( + const tf2::Transform & prev, const tf2::Transform & cur, const tf2::Transform & next) +{ + return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next)); +} + +/** + * Checks the given plan. The first and last poses are always accepted (if they exist). Intermediate + * poses are only accepted if the angle (with respect to the previous and next poses) is obtuse. + * + * @param[in] plan Plan to check. + * @return True if all poses in the plan are accepted. False otherwise. + */ +bool check_plan(const std::vector & plan) +{ + const auto plan_size = plan.size(); + + for (int pose_idx = 1; pose_idx < static_cast(plan_size) - 1; ++pose_idx) { + const auto & prev_pose = plan[pose_idx - 1]; + const auto & pose = plan[pose_idx]; + const auto & next_pose = plan[pose_idx + 1]; + if (!is_pose_angle_obtuse(prev_pose, pose, next_pose)) { + return false; + } + } + + return true; +} + +} // namespace + +void Controller::setHolonomic(bool holonomic) +{ + // Set configuration parameters + ROS_WARN_COND(holonomic, "Holonomic mode is unmaintained. Expect bugs with y-direction"); + holonomic_robot_enable_ = holonomic; +} + +void Controller::setEstimatePoseAngle(bool estimate_pose_angle) +{ + // Set configuration parameters + estimate_pose_angle_ = estimate_pose_angle; +} + +void Controller::setTricycleModel( + bool tricycle_model_enabled, const tf2::Transform & tf_base_to_steered_wheel) +{ + // Set tricycle model + use_tricycle_model_ = tricycle_model_enabled; + tf_base_to_steered_wheel_ = tf_base_to_steered_wheel; + const double wheel_x = tf_base_to_steered_wheel_.getOrigin().x(); + const double wheel_y = tf_base_to_steered_wheel_.getOrigin().y(); + + const double distance_base_to_steered_wheel = hypot(wheel_x, wheel_y); + const double wheel_theta = atan2(wheel_y, wheel_x); + inverse_kinematics_matrix_[0][0] = 1; + inverse_kinematics_matrix_[0][1] = -distance_base_to_steered_wheel * sin(wheel_theta); + inverse_kinematics_matrix_[1][0] = 0; + inverse_kinematics_matrix_[1][1] = -distance_base_to_steered_wheel * cos(wheel_theta); + + const double determinant = inverse_kinematics_matrix_[0][0] * inverse_kinematics_matrix_[1][1] - + inverse_kinematics_matrix_[0][1] * inverse_kinematics_matrix_[1][0]; + + if (determinant == 0) { + ROS_ERROR("Steered wheel at base_link. Invalid for tricycle model, it will be disabled."); + use_tricycle_model_ = false; + return; + } + + forward_kinematics_matrix_[0][0] = inverse_kinematics_matrix_[1][1] / determinant; + forward_kinematics_matrix_[0][1] = -inverse_kinematics_matrix_[0][1] / determinant; + forward_kinematics_matrix_[1][0] = -inverse_kinematics_matrix_[1][0] / determinant; + forward_kinematics_matrix_[1][1] = inverse_kinematics_matrix_[0][0] / determinant; + + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel_.getRotation()); +} + +geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics( + double x_vel, double steering_angle) +{ + geometry_msgs::Twist estimated_base_twist; + const double x_alpha = x_vel * cos(steering_angle); + const double y_alpha = x_vel * sin(steering_angle); + + estimated_base_twist.linear.x = + forward_kinematics_matrix_[0][0] * x_alpha + forward_kinematics_matrix_[0][1] * y_alpha; + estimated_base_twist.angular.z = + forward_kinematics_matrix_[1][0] * x_alpha + forward_kinematics_matrix_[1][1] * y_alpha; + + return estimated_base_twist; +} + +TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics( + const geometry_msgs::Twist & cmd_vel) +{ + TricycleSteeringCmdVel steering_cmd_vel; + const double x_alpha = inverse_kinematics_matrix_[0][0] * cmd_vel.linear.x + + inverse_kinematics_matrix_[0][1] * cmd_vel.angular.z; + const double y_alpha = inverse_kinematics_matrix_[1][0] * cmd_vel.linear.x + + inverse_kinematics_matrix_[1][1] * cmd_vel.angular.z; + + steering_cmd_vel.steering_angle = atan2(y_alpha, x_alpha); + steering_cmd_vel.speed = hypot(x_alpha, y_alpha); + + return steering_cmd_vel; +} + +bool Controller::setPlan( + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const std::vector & global_plan) +{ + ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan.size()); + + if (!check_plan(global_plan)) { + ROS_ERROR("Rejected plan because not all poses were in the expected direction of the path!"); + return false; + } + + global_plan_tf_ = global_plan; + + if (!config_.track_base_link) { + // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) + tf2::Transform carrotTF( + tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), + tf2::Vector3(config_.l, 0.0, 0.0)); + global_plan_tf_.push_back(global_plan_tf_.back() * carrotTF); + } + + // Whenever a new path is recieved, computed the closest pose to + // the current carrot pose + controller_state_.current_global_plan_index = 0; + + // find closest current position to global plan + double minimum_distance_to_path = 1e3; + // We define segment0 to be the segment connecting pose0 and pose1. + // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. + for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { + /* Get distance to segment to determine if this is the segment to start at */ + const auto dist_to_segment = distSquared( + current_tf, closestPoseOnSegment( + current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1], + estimate_pose_angle_)); + // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose + // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! + if (dist_to_segment < minimum_distance_to_path) { + minimum_distance_to_path = dist_to_segment; + controller_state_.current_global_plan_index = idx_path; + } + } + + const auto deltas = deltas_of_plan(global_plan_tf_); + + distance_to_goal_vector_ = distances_to_goal(deltas); + turning_radius_inv_vector_ = inverse_turning_radiuses(deltas); + + assert(global_plan_tf_.size() == distance_to_goal_vector_.size()); + assert(global_plan_tf_.size() == turning_radius_inv_vector_.size()); + + // Set initial velocity + switch (config_.init_vel_method) { + case Pid_Zero: + reset(); + break; + case Pid_Odom: + reset(); + controller_state_.current_x_vel = odom_twist.linear.x; + controller_state_.current_yaw_vel = odom_twist.angular.z; + ROS_INFO( + "Resuming on odom velocity x: %f, yaw: %f", odom_twist.linear.x, odom_twist.angular.z); + break; + default: + ROS_DEBUG("Internal controller_state stays valid"); + break; + } + + // When velocity error is too big reset current_x_vel + if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > config_.max_error_x_vel) { + // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here + ROS_WARN( + "Large control error. Current_x_vel %f / odometry %f", controller_state_.current_x_vel, + odom_twist.linear.x); + } + controller_state_.end_phase_enabled = false; + controller_state_.end_reached = false; + + return true; +} + +bool Controller::setPlan( + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const tf2::Transform & tf_base_to_steered_wheel, + const geometry_msgs::Twist & /* steering_odom_twist */, + const std::vector & global_plan) +{ + const auto result = setPlan(current_tf, odom_twist, global_plan); + + if (result) { + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.getRotation()); + } + + return result; +} + +Controller::FindPoseOnPlanResult Controller::findPoseOnPlan( + const tf2::Transform & current_tf, std::size_t & global_plan_index) const +{ + auto current_tf2 = current_tf; + // 'Project' current_tf by removing z-component + tf2::Vector3 originProj = current_tf2.getOrigin(); + originProj.setZ(0.0); + current_tf2.setOrigin(originProj); + + // Computed the closest pose to the current provided pose + // by looking on the surroundings of the last known pose + + // find closest current position to global plan + double minimum_distance_to_path = FLT_MAX; + double distance_to_path; + tf2::Transform error; + + // We define segment0 to be the segment connecting pose0 and pose1. + // Hence, when idx_path==i we are currently tracking the section connection pose i and pose i+1 + + // First look in current position and in front + for (auto idx_path = global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) { + error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); + // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose + // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! + distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); + + if (distance_to_path <= minimum_distance_to_path) { + minimum_distance_to_path = distance_to_path; + global_plan_index = idx_path; + } else { + break; + } + } + + // Then look backwards + for (auto idx_path = global_plan_index; idx_path > 0; --idx_path) { + error = current_tf2.inverseTimes(global_plan_tf_[idx_path - 1]); + // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose + // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! + distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); + + if (distance_to_path < minimum_distance_to_path) { + minimum_distance_to_path = distance_to_path; + global_plan_index = idx_path - 1; + } else { + break; + } + } + ROS_DEBUG("progress: %lu of %lu", global_plan_index, global_plan_tf_.size() - 1); + // To finalize, compute the indexes of the start and end points of + // the closest line segment to the current carrot + + if (global_plan_index == 0) { + const auto closest_pose = closestPoseOnSegment( + current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); + const auto distance_to_goal = + distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); + + return {closest_pose, global_plan_index, distance_to_goal, 0}; + } + + if (global_plan_index == global_plan_tf_.size() - 1) { + const auto closest_pose = closestPoseOnSegment( + current_tf2, global_plan_tf_[global_plan_index - 1], global_plan_tf_[global_plan_index], + estimate_pose_angle_); + const auto distance_to_goal = + sqrt(distSquared(global_plan_tf_[global_plan_index], closest_pose)); + + return {closest_pose, global_plan_index - 1, distance_to_goal, global_plan_tf_.size() - 2}; + } + + const auto closest_pose_ahead = closestPoseOnSegment( + current_tf2, global_plan_tf_[global_plan_index], global_plan_tf_[global_plan_index + 1], + estimate_pose_angle_); + const auto closest_pose_behind = closestPoseOnSegment( + current_tf2, global_plan_tf_[global_plan_index - 1], global_plan_tf_[global_plan_index], + estimate_pose_angle_); + + if ( + distSquared(current_tf2, closest_pose_ahead) < distSquared(current_tf2, closest_pose_behind)) { + const auto distance_to_goal = + distance_to_goal_vector_[global_plan_index + 1] + + sqrt(distSquared(global_plan_tf_[global_plan_index + 1], closest_pose_ahead)); + + return {closest_pose_ahead, global_plan_index, distance_to_goal, global_plan_index}; + } + + const auto distance_to_goal = + distance_to_goal_vector_[global_plan_index] + + sqrt(distSquared(global_plan_tf_[global_plan_index], closest_pose_behind)); + + return {closest_pose_behind, global_plan_index, distance_to_goal, global_plan_index - 1}; +} + +Controller::UpdateResult Controller::update( + double target_x_vel, double target_end_x_vel, const tf2::Transform & current_tf, + const geometry_msgs::Twist & odom_twist, ros::Duration dt) +{ + UpdateResult result; + + current_with_carrot_ = getControlPointPose(current_tf, config_.l); + + const auto & reference_pose = config_.track_base_link ? current_tf : current_with_carrot_; + const auto find_result = + findPoseOnPlan(reference_pose, controller_state_.current_global_plan_index); + const auto & path_pose_idx = find_result.path_pose_idx; + const auto & distance_to_goal = find_result.distance_to_goal; + + current_pos_on_plan_ = current_goal_ = find_result.pose; + + if (config_.track_base_link) { + current_goal_ = getControlPointPose(current_goal_, config_.l); + } + + result.progress = 1.0 - distance_to_goal / distance_to_goal_vector_[0]; + + // Compute errorPose between controlPose and currentGoalPose + const auto error = current_with_carrot_.inverseTimes(current_goal_); + + //***** Feedback control *****// + if (!((config_.Kp_lat <= 0. && config_.Ki_lat <= 0. && config_.Kd_lat <= 0.) || + (config_.Kp_lat >= 0. && config_.Ki_lat >= 0. && + config_.Kd_lat >= 0.))) // All 3 gains should have the same sign + { + ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); + } + if (!((config_.Kp_ang <= 0. && config_.Ki_ang <= 0. && config_.Kd_ang <= 0.) || + (config_.Kp_ang >= 0. && config_.Ki_ang >= 0. && + config_.Kd_ang >= 0.))) // All 3 gains should have the same sign + { + ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); + } + + auto error_lat_filtered = controller_state_.error_lat.filter(error.getOrigin().y(), dt.toSec()); + auto error_ang_filtered = controller_state_.error_ang.filter( + angles::normalize_angle(tf2::getYaw(error.getRotation())), dt.toSec()); + + // tracking error for diagnostic purposes + // Transform current pose into local-path-frame to get tracked-frame-error + tf2::Quaternion path_quat; + path_quat.setEuler( + 0.0, 0.0, + atan2( + global_plan_tf_[path_pose_idx + 1].getOrigin().y() - + global_plan_tf_[path_pose_idx].getOrigin().y(), + global_plan_tf_[path_pose_idx + 1].getOrigin().x() - + global_plan_tf_[path_pose_idx].getOrigin().x())); + tf2::Transform path_segmen_tf = tf2::Transform( + path_quat, tf2::Vector3( + global_plan_tf_[path_pose_idx].getOrigin().x(), + global_plan_tf_[path_pose_idx].getOrigin().y(), + global_plan_tf_[path_pose_idx].getOrigin().z())); + + tf2::Vector3 current_tracking_err = -(path_segmen_tf.inverse() * current_tf.getOrigin()); + + // trackin_error here represents the error between tracked link and position on plan + controller_state_.tracking_error_lat = current_tracking_err.y(); + controller_state_.tracking_error_ang = angles::normalize_angle(tf2::getYaw(error.getRotation())), + dt.toSec(); + + // integrate the error + auto error_integral_lat = + controller_state_.error_integral_lat.filter(error_lat_filtered, dt.toSec()); + auto error_integral_ang = + controller_state_.error_integral_ang.filter(error_lat_filtered, dt.toSec()); + + // Take derivative of error, first the raw unfiltered data: + auto error_deriv_lat = controller_state_.error_deriv_lat.filter(error_lat_filtered, dt.toSec()); + auto error_deriv_ang = controller_state_.error_deriv_ang.filter(error_ang_filtered, dt.toSec()); + + // calculate the control effort + const auto proportional_lat = config_.Kp_lat * error_lat_filtered; + const auto integral_lat = config_.Ki_lat * error_integral_lat; + const auto derivative_lat = config_.Kd_lat * error_deriv_lat; + + const auto proportional_ang = config_.Kp_ang * error_ang_filtered; + const auto integral_ang = config_.Ki_ang * error_integral_ang; + const auto derivative_ang = config_.Kd_ang * error_deriv_ang; + + /***** Compute forward velocity *****/ + // Apply acceleration limits and end velocity + double t_end_phase_current; + double d_end_phase; + + const double current_x_vel = controller_state_.current_x_vel; + const double current_yaw_vel = controller_state_.current_yaw_vel; + + // Compute time to reach end velocity from current velocity + // Compute estimate overall distance during end_phase + // The estimates are done a bit conservative to account that robot will take longer + // to de-accelerate and thus avoid abrupt velocity changes at the end of the trajectory + // The sample time plays an important role on how good these estimates are. + // Thus We add a distance to the end phase distance estimation depending on the sample time + if ( + (current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || + (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) { + t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_decc); + } else { + t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_acc); + } + d_end_phase = (current_x_vel + target_end_x_vel) * 0.5 * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); + ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); + ROS_DEBUG("d_end_phase: %f", d_end_phase); + ROS_DEBUG("distance_to_goal: %f", distance_to_goal); + + const auto in_direction_of_goal = + is_in_direction_of_target(current_tf, current_goal_.getOrigin(), target_x_vel); + + // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. + // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. + // This is to avoid skipping paths that start with opposite velocity. + if ((distance_to_goal <= fabs(d_end_phase)) && in_direction_of_goal) { + // This state will be remebered to avoid jittering on target_x_vel + controller_state_.end_phase_enabled = true; + } + + if (controller_state_.end_phase_enabled && fabs(target_x_vel) > VELOCITY_EPS) { + current_target_x_vel_ = target_end_x_vel; + } else { + controller_state_.end_phase_enabled = false; + current_target_x_vel_ = target_x_vel; + } + + // Determine if we need to accelerate, decelerate or maintain speed + double current_target_acc = 0; // Assume maintaining speed + if (fabs(current_target_x_vel_) <= VELOCITY_EPS) // Zero velocity requested + { + if (current_x_vel > current_target_x_vel_) { + current_target_acc = -config_.target_x_decc; + } else { + current_target_acc = config_.target_x_decc; + } + } else if (current_target_x_vel_ > 0) // Positive velocity requested + { + if (current_x_vel > current_target_x_vel_) { + current_target_acc = -config_.target_x_decc; + } else { + current_target_acc = config_.target_x_acc; + } + } else // Negative velocity requested + { + if (current_x_vel > current_target_x_vel_) { + current_target_acc = -config_.target_x_acc; + } else { + current_target_acc = config_.target_x_decc; + } + } + + const double acc_desired = (current_target_x_vel_ - current_x_vel) / dt.toSec(); + const double acc_abs = fmin(fabs(acc_desired), fabs(current_target_acc)); + const auto acc = copysign(acc_abs, current_target_acc); + + double new_x_vel = current_x_vel + acc * dt.toSec(); + + // For low target_end_x_vel we have a minimum velocity to ensure the goal is reached + double min_vel = copysign(1.0, config_.l) * config_.abs_minimum_x_vel; + if ( + !controller_state_.end_reached && controller_state_.end_phase_enabled && + fabs(target_end_x_vel) <= fabs(min_vel) + VELOCITY_EPS && + fabs(new_x_vel) <= fabs(min_vel) + VELOCITY_EPS) { + new_x_vel = min_vel; + } + + // When velocity error is too big reset current_x_vel + if ( + fabs(odom_twist.linear.x) < fabs(current_target_x_vel_) && + fabs(odom_twist.linear.x - new_x_vel) > config_.max_error_x_vel) { + // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here + ROS_WARN_THROTTLE( + 1.0, "Large tracking error. Current_x_vel %f / odometry %f", new_x_vel, odom_twist.linear.x); + } + + // Force target_end_x_vel at the very end of the path + // Or when the end velocity is reached. + // Warning! If target_end_x_vel == 0 and min_vel = 0 then the robot might not reach end pose + if ( + (distance_to_goal == 0.0 && target_end_x_vel >= VELOCITY_EPS) || + (controller_state_.end_phase_enabled && new_x_vel >= target_end_x_vel - VELOCITY_EPS && + new_x_vel <= target_end_x_vel + VELOCITY_EPS)) { + controller_state_.end_reached = true; + controller_state_.end_phase_enabled = false; + result.progress = 1.0; + result.eda = 0.0; + enabled_ = false; + } else { + controller_state_.end_reached = false; + // eda (Estimated duration of arrival) estimation + if (fabs(target_x_vel) > VELOCITY_EPS) { + const double t_const = + (copysign(distance_to_goal, target_x_vel) - d_end_phase) / target_x_vel; + result.eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); + } else { + result.eda = LONG_DURATION; + } + } + /******* end calculation of forward velocity ********/ + + //***** Overall control *****// + // Controller logic && overall control effort + control_effort_long_ = new_x_vel; + control_effort_lat_ = 0.0; + control_effort_ang_ = 0.0; + + if (config_.feedback_lat) { + control_effort_lat_ = proportional_lat + integral_lat + derivative_lat; + } + if (config_.feedback_ang) { + control_effort_ang_ = proportional_ang + integral_ang + derivative_ang; + } + + //***** Feedforward control *****// + if (config_.feedforward_lat) { + feedforward_lat_ = 0.0; // Not implemented + control_effort_lat_ = control_effort_lat_ + feedforward_lat_; + } else { + feedforward_lat_ = 0.0; + } + + if (config_.feedforward_ang) { + feedforward_ang_ = + turning_radius_inv_vector_[find_result.last_visited_pose_index] * control_effort_long_; + ROS_DEBUG( + "turning_radius_inv_vector[%lu] = %f", find_result.last_visited_pose_index, + turning_radius_inv_vector_[find_result.last_visited_pose_index]); + + control_effort_ang_ = control_effort_ang_ + feedforward_ang_; + } else { + feedforward_ang_ = 0.0; + } + + // Apply saturation limits + control_effort_lat_ = std::clamp(control_effort_lat_, lat_lower_limit, lat_upper_limit); + control_effort_ang_ = std::clamp(control_effort_ang_, ang_lower_limit, ang_upper_limit); + + // Populate debug output + // Error topic containing the 'control' error on which the PID acts + result.pid_debug.control_error.linear.x = 0.0; + result.pid_debug.control_error.linear.y = error_lat_filtered; + result.pid_debug.control_error.angular.z = error_ang_filtered; + // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link + result.pid_debug.tracking_error.linear.x = 0.0; + result.pid_debug.tracking_error.linear.y = controller_state_.tracking_error_lat; + result.pid_debug.tracking_error.angular.z = controller_state_.tracking_error_ang; + + result.pid_debug.proportional.linear.x = 0.0; + result.pid_debug.proportional.linear.y = proportional_lat; + result.pid_debug.proportional.angular.z = proportional_ang; + + result.pid_debug.integral.linear.x = 0.0; + result.pid_debug.integral.linear.y = integral_lat; + result.pid_debug.integral.angular.z = integral_ang; + + result.pid_debug.derivative.linear.x = 0.0; + result.pid_debug.derivative.linear.y = derivative_lat; + result.pid_debug.derivative.angular.z = derivative_ang; + + result.pid_debug.feedforward.linear.x = new_x_vel; + result.pid_debug.feedforward.linear.y = feedforward_lat_; + result.pid_debug.feedforward.angular.z = feedforward_ang_; + + // Generate twist message + if (holonomic_robot_enable_) { + result.velocity_command.linear.x = control_effort_long_; + result.velocity_command.linear.y = control_effort_lat_; + result.velocity_command.linear.z = 0; + result.velocity_command.angular.x = 0; + result.velocity_command.angular.y = 0; + result.velocity_command.angular.z = control_effort_ang_; + result.velocity_command.angular.z = + std::clamp(result.velocity_command.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); + } else { + result.velocity_command.linear.x = control_effort_long_; + result.velocity_command.linear.y = 0; + result.velocity_command.linear.z = 0; + result.velocity_command.angular.x = 0; + result.velocity_command.angular.y = 0; + result.velocity_command.angular.z = + copysign(1.0, config_.l) * control_effort_lat_ + + control_effort_ang_; // Take the sign of l for the lateral control effort + result.velocity_command.angular.z = + std::clamp(result.velocity_command.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); + // For non-holonomic robots apply saturation based on minimum turning radius + double max_ang_twist_tr; + if (config_.min_turning_radius < RADIUS_EPS) { + // Rotation in place is allowed + // minimum_turning_radius = RADIUS_EPS; // This variable is not used anymore so it does not matter + // do not restrict angular velocity. Thus use the biggets number possible + max_ang_twist_tr = std::numeric_limits::infinity(); + } else { + max_ang_twist_tr = fabs(result.velocity_command.linear.x / config_.min_turning_radius); + } + result.velocity_command.angular.z = + std::clamp(result.velocity_command.angular.z, -max_ang_twist_tr, max_ang_twist_tr); + } + // Apply max acceleration limit to yaw + const double yaw_acc = std::clamp( + (result.velocity_command.angular.z - current_yaw_vel) / dt.toSec(), -config_.max_yaw_acc, + config_.max_yaw_acc); + const double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); + result.velocity_command.angular.z = new_yaw_vel; + + // Transform velocity commands at base_link to steer when using tricycle model + if (use_tricycle_model_) { + geometry_msgs::Twist output_steering; + TricycleSteeringCmdVel steering_cmd = + computeTricycleModelInverseKinematics(result.velocity_command); + if (result.velocity_command.linear.x < 0.0 && steering_cmd.speed > 0.0) { + steering_cmd.speed = -steering_cmd.speed; + if (steering_cmd.steering_angle > 0) { + steering_cmd.steering_angle = steering_cmd.steering_angle - M_PI; + } else { + steering_cmd.steering_angle = steering_cmd.steering_angle + M_PI; + } + } + // Apply limits to steering commands + steering_cmd.steering_angle = std::clamp( + steering_cmd.steering_angle, -config_.max_steering_angle, config_.max_steering_angle); + const double steering_yaw_vel = std::clamp( + (steering_cmd.steering_angle - controller_state_.previous_steering_angle) / dt.toSec(), + -config_.max_steering_yaw_vel, config_.max_steering_yaw_vel); + const double steering_angle_acc = std::clamp( + (steering_yaw_vel - controller_state_.previous_steering_yaw_vel) / dt.toSec(), + -config_.max_steering_yaw_acc, config_.max_steering_yaw_acc); + steering_cmd.steering_angle_velocity = + controller_state_.previous_steering_yaw_vel + (steering_angle_acc * dt.toSec()); + steering_cmd.steering_angle = controller_state_.previous_steering_angle + + (steering_cmd.steering_angle_velocity * dt.toSec()); + + steering_cmd.speed = + std::clamp(steering_cmd.speed, -config_.max_steering_x_vel, config_.max_steering_x_vel); + steering_cmd.acceleration = std::clamp( + (steering_cmd.speed - controller_state_.previous_steering_x_vel) / dt.toSec(), + -config_.max_steering_x_acc, config_.max_steering_x_acc); + steering_cmd.speed = + controller_state_.previous_steering_x_vel + (steering_cmd.acceleration * dt.toSec()); + + controller_state_.previous_steering_angle = steering_cmd.steering_angle; + controller_state_.previous_steering_yaw_vel = steering_cmd.steering_angle_velocity; + controller_state_.previous_steering_x_vel = steering_cmd.speed; + + // Compute velocities back to base_link and update controller state + output_steering = + computeTricycleModelForwardKinematics(steering_cmd.speed, steering_cmd.steering_angle); + controller_state_.current_x_vel = output_steering.linear.x; + controller_state_.current_yaw_vel = output_steering.angular.z; + + result.pid_debug.steering_angle = steering_cmd.steering_angle; + result.pid_debug.steering_yaw_vel = steering_cmd.steering_angle_velocity; + result.pid_debug.steering_x_vel = steering_cmd.speed; + + result.velocity_command = output_steering; + } + + // Publish control effort if controller enabled + if (!enabled_) // Do nothing reset integral action and all filters + { + controller_state_.error_integral_lat.reset(); + controller_state_.error_integral_ang.reset(); + } + + controller_state_.current_x_vel = new_x_vel; + controller_state_.current_yaw_vel = new_yaw_vel; + + return result; +} + +Controller::UpdateResult Controller::update_with_limits( + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt) +{ + // All limits are absolute + double max_x_vel = std::abs(config_.target_x_vel); + + // Apply external limit + max_x_vel = std::min(max_x_vel, vel_max_external_); + + // Apply obstacle limit + max_x_vel = std::min(max_x_vel, vel_max_obstacle_); + + // Apply mpc limit (last because less iterations required if max vel is already limited) + double vel_max_mpc = std::numeric_limits::infinity(); + if (config_.use_mpc) { + vel_max_mpc = std::abs( + mpc_based_max_vel(std::copysign(max_x_vel, config_.target_x_vel), current_tf, odom_twist)); + max_x_vel = std::min(max_x_vel, vel_max_mpc); + } + + // Some logging: + ROS_DEBUG( + "max_x_vel=%.3f, target_x_vel=%.3f, vel_max_external=%.3f, vel_max_obstacle=%.3f, " + "vel_max_mpc=%.3f", + max_x_vel, config_.target_x_vel, vel_max_external_, vel_max_obstacle_, vel_max_mpc); + if (max_x_vel != config_.target_x_vel) { + if (max_x_vel == vel_max_external_) { + ROS_WARN_THROTTLE(5.0, "External velocity limit active %.2fm/s", vel_max_external_); + } else if (max_x_vel == vel_max_obstacle_) { + ROS_WARN_THROTTLE(5.0, "Obstacle velocity limit active %.2fm/s", vel_max_obstacle_); + } else if (max_x_vel == vel_max_mpc) { + ROS_WARN_THROTTLE(5.0, "MPC velocity limit active %.2fm/s", vel_max_mpc); + } + } + + // The end velocity is bound by the same limits to avoid accelerating above the limit in the end phase + double max_end_x_vel = std::min( + {std::abs(config_.target_end_x_vel), vel_max_external_, vel_max_obstacle_, vel_max_mpc}); + max_end_x_vel = std::copysign(max_end_x_vel, config_.target_end_x_vel); + + // Update the controller with the new setting + max_x_vel = std::copysign(max_x_vel, config_.target_x_vel); + return update(max_x_vel, max_end_x_vel, current_tf, odom_twist, dt); +} + +// output updated velocity command: (Current position, current measured velocity, closest point index, estimated +// duration of arrival, debug info) +double Controller::mpc_based_max_vel( + double target_x_vel, const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist) +{ + // Temporary save global data + ControllerState controller_state_saved; + controller_state_saved = controller_state_; + + // Bisection optimisation parameters + double target_x_vel_prev = 0.0; // Previous iteration velocity command + int mpc_vel_optimization_iter = 0; + + // MPC parameters + int mpc_fwd_iter = 0; // Reset MPC iterations + + // Create predicted position vector + auto predicted_tf = current_tf; + geometry_msgs::Twist pred_twist = odom_twist; + + double new_nominal_x_vel = target_x_vel; // Start off from the current velocity + + // Loop MPC + while (mpc_fwd_iter < config_.mpc_max_fwd_iterations && + mpc_vel_optimization_iter <= config_.mpc_max_vel_optimization_iterations) { + mpc_fwd_iter += 1; + + // Check if robot stays within bounds for all iterations, if the new_nominal_x_vel is smaller than + // max_target_x_vel we can increase it + if ( + mpc_fwd_iter == config_.mpc_max_fwd_iterations && + fabs(controller_state_.tracking_error_lat) <= config_.mpc_max_error_lat && + fabs(new_nominal_x_vel) < abs(target_x_vel)) { + mpc_vel_optimization_iter += 1; + + // When we reach the maximum allowed mpc optimization iterations, do not change velocity anymore + if (mpc_vel_optimization_iter > config_.mpc_max_vel_optimization_iterations) { + break; + } + + // Increase speed + target_x_vel_prev = std::exchange( + new_nominal_x_vel, + copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + + new_nominal_x_vel); + + // Reset variables + controller_state_ = controller_state_saved; + + predicted_tf = current_tf; + pred_twist = odom_twist; + mpc_fwd_iter = 0; + } + // If the robot gets out of bounds earlier we decrease the velocity + else if (abs(controller_state_.tracking_error_lat) >= config_.mpc_max_error_lat) { + mpc_vel_optimization_iter += 1; + + // Lower speed + target_x_vel_prev = std::exchange( + new_nominal_x_vel, + -copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + + new_nominal_x_vel); + + // Reset variables + controller_state_ = controller_state_saved; + + predicted_tf = current_tf; + pred_twist = odom_twist; + mpc_fwd_iter = 0; + + // Warning if new_nominal_x_vel becomes really low + if (abs(new_nominal_x_vel) < 0.01) { + ROS_WARN_THROTTLE(5.0, "Lowering velocity did not decrease the lateral error enough."); + } + } else if (mpc_fwd_iter != config_.mpc_max_fwd_iterations) { + // Run controller + // Output: pred_twist.[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z] + pred_twist = Controller::update( + new_nominal_x_vel, config_.target_end_x_vel, predicted_tf, pred_twist, + ros::Duration(config_.mpc_simulation_sample_time)) + .velocity_command; + + // Run plant model + const double theta = tf2::getYaw(predicted_tf.getRotation()); + predicted_tf.getOrigin().setX( + predicted_tf.getOrigin().getX() + + pred_twist.linear.x * cos(theta) * config_.mpc_simulation_sample_time); + predicted_tf.getOrigin().setY( + predicted_tf.getOrigin().getY() + + pred_twist.linear.x * sin(theta) * config_.mpc_simulation_sample_time); + tf2::Quaternion q; + q.setRPY(0, 0, theta + pred_twist.angular.z * config_.mpc_simulation_sample_time); + predicted_tf.setRotation(q); + } + } + // Apply limits to the velocity + double mpc_vel_limit = + copysign(1.0, new_nominal_x_vel) * fmax(fabs(new_nominal_x_vel), config_.mpc_min_x_vel); + + // Revert global variables + controller_state_ = controller_state_saved; + + return std::abs(mpc_vel_limit); +} + +void Controller::printParameters() const +{ + ROS_INFO("CONTROLLER PARAMETERS"); + ROS_INFO("-----------------------------------------"); + ROS_INFO("Controller enabled: %i", enabled_); + ROS_INFO("Controller DEBUG enabled: %i", config_.controller_debug_enabled); + ROS_INFO("Distance L: %f", config_.l); + ROS_INFO("Track base_link enabled?: %i", config_.track_base_link); + + ROS_INFO( + "Target forward velocities (xv: %f, xv_end,: %f)", config_.target_x_vel, + config_.target_end_x_vel); + ROS_INFO( + "Target forward (de)accelerations (xacc: %f, xdecc,: %f)", config_.target_x_acc, + config_.target_x_decc); + ROS_INFO("Maximum allowed forward velocity error: %f", config_.max_error_x_vel); + ROS_INFO("Feedback (lat, ang): ( %i, %i)", config_.feedback_lat, config_.feedback_ang); + ROS_INFO("Feedforward (lat, ang): (%i, %i)", config_.feedforward_lat, config_.feedforward_ang); + ROS_INFO( + "Lateral gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_lat, config_.Ki_lat, config_.Kd_lat); + ROS_INFO( + "Angular gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_ang, config_.Ki_ang, config_.Kd_ang); + + ROS_INFO("Robot type (holonomic): (%i)", holonomic_robot_enable_); + + ROS_INFO("Integral-windup limit: %f", windup_limit); + ROS_INFO("Saturation limits xy: %f/%f", lat_upper_limit, lat_lower_limit); + ROS_INFO("Saturation limits ang: %f/%f", ang_upper_limit, ang_lower_limit); + ROS_INFO("-----------------------------------------"); +} + +void Controller::configure(path_tracking_pid::PidConfig & config) +{ + controller_state_.error_lat.configure(config.lowpass_cutoff, config.lowpass_damping); + controller_state_.error_ang.configure(config.lowpass_cutoff, config.lowpass_damping); + controller_state_.error_integral_lat.configure(windup_limit); + controller_state_.error_integral_ang.configure(windup_limit); + + // Erase all queues when config changes + controller_state_.error_lat.reset(); + controller_state_.error_deriv_lat.reset(); + + controller_state_.error_ang.reset(); + controller_state_.error_deriv_ang.reset(); + + config.l = copysign(config.l, config.target_x_vel); + if (controller_state_.end_phase_enabled) { + ROS_WARN_COND( + abs(config.target_end_x_vel - config_.target_end_x_vel) > 1e-3, + "Won't change end velocity in end phase"); + ROS_WARN_COND( + abs(config.target_x_acc - config_.target_x_acc) > 1e-3, + "Won't change accelerations in end phase"); + ROS_WARN_COND( + abs(config.target_x_decc - config_.target_x_decc) > 1e-3, + "Won't change accelerations in end phase"); + config.target_end_x_vel = config_.target_end_x_vel; + config.target_x_acc = config_.target_x_acc; + config.target_x_decc = config_.target_x_decc; + } + + config.groups.mpc_group.state = config.use_mpc; // Hide config options if disabled + config.mpc_min_x_vel = fmin(config.mpc_min_x_vel, fabs(config.target_x_vel)); + + config.groups.collision_group.state = config.anti_collision; // Hide config options if disabled + + config_ = config; + + ROS_DEBUG( + "Track base_link? Then global path poses are needed! '%d'", (int)config_.track_base_link); + + // printParameters(); +} + +path_tracking_pid::PidConfig Controller::getConfig() { return config_; } + +void Controller::setEnabled(bool value) +{ + ROS_DEBUG("Controller::setEnabled(%d)", value); + enabled_ = value; +} + +void Controller::reset() +{ + controller_state_.current_x_vel = 0.0; + controller_state_.current_yaw_vel = 0.0; + controller_state_.previous_steering_angle = 0.0; + controller_state_.previous_steering_yaw_vel = 0.0; + controller_state_.previous_steering_x_vel = 0.0; + controller_state_.error_lat.reset(); + controller_state_.error_ang.reset(); + controller_state_.error_integral_lat.reset(); + controller_state_.error_integral_ang.reset(); + controller_state_.error_deriv_lat.reset(); + controller_state_.error_deriv_ang.reset(); +} + +void Controller::setVelMaxExternal(double value) +{ + if (value < 0.0) { + ROS_ERROR_THROTTLE(1.0, "External velocity limit (%f) has to be positive", value); + return; + } + if (value < 0.1) { + ROS_WARN_THROTTLE( + 1.0, "External velocity limit is very small (%f), this could result in standstill", value); + } + vel_max_external_ = value; +} + +void Controller::setVelMaxObstacle(double value) +{ + ROS_WARN_COND( + vel_max_obstacle_ != 0.0 && value == 0.0, "Collision imminent, slamming the brakes"); + vel_max_obstacle_ = value; +} + +double Controller::getVelMaxObstacle() const { return vel_max_obstacle_; } + +} // namespace path_tracking_pid diff --git a/src/details/derivative.cpp b/src/details/derivative.cpp new file mode 100644 index 00000000..25dc4f63 --- /dev/null +++ b/src/details/derivative.cpp @@ -0,0 +1,13 @@ +#include + +namespace path_tracking_pid::details +{ +double Derivative::filter(double u, double step_size) +{ + // save history + u_.push(u); + return (u_[0] - u_[1]) / step_size; +} + +void Derivative::reset() { u_ = {}; } +} // namespace path_tracking_pid::details diff --git a/src/details/integral.cpp b/src/details/integral.cpp new file mode 100644 index 00000000..3b8997cb --- /dev/null +++ b/src/details/integral.cpp @@ -0,0 +1,28 @@ +#include + +namespace path_tracking_pid::details +{ +Integral::Integral(double windup_limit) : windup_limit_(windup_limit) {} + +void Integral::configure(double windup_limit) { windup_limit_ = windup_limit; } + +double Integral::filter(double u, double step_size) +{ + // save history + u_.push(u); + y_.push(u); // increase index so the math below looks correct + + // A continous time integrator was discretized with Tustin's method. For a mathematical + // explanation, see doc/integral_tustin.ipynb + auto T = step_size; + y_[0] = T / 2 * (u_[0] + u_[1]) + y_[1]; + y_[0] = std::clamp(y_[0], -windup_limit_, windup_limit_); + return y_[0]; +} + +void Integral::reset() +{ + u_ = {}; + y_ = {}; +} +} // namespace path_tracking_pid::details diff --git a/src/details/second_order_lowpass.cpp b/src/details/second_order_lowpass.cpp new file mode 100644 index 00000000..3da54727 --- /dev/null +++ b/src/details/second_order_lowpass.cpp @@ -0,0 +1,45 @@ +#include +#include + +namespace path_tracking_pid::details +{ +SecondOrderLowpass::SecondOrderLowpass(double cutoff, double damping) +: cutoff_(cutoff), damping_(damping) +{ +} + +void SecondOrderLowpass::configure(double cutoff, double damping) +{ + cutoff_ = cutoff; + damping_ = damping; +} + +double SecondOrderLowpass::filter(double u, double step_size) +{ + // save history + u_.push(u); + y_.push(u); // increase index so the math below looks correct + + if (cutoff_ == 0) { + return u; + } + + // A continous time second order lowpass was discretized with Tustin's method. For a mathematical + // explanation, see doc/second_order_lowpass_tustin.ipynb + auto c = cutoff_; + auto d = damping_; + auto T = step_size; + auto a = 2 * M_PI * c; + auto b = T * a; + y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] - + (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) / + (pow(b, 2) + 4 * T * a * d + 4); + return y_[0]; +} + +void SecondOrderLowpass::reset() +{ + u_ = {}; + y_ = {}; +} +} // namespace path_tracking_pid::details diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp new file mode 100644 index 00000000..49a48dbe --- /dev/null +++ b/src/path_tracking_pid_local_planner.cpp @@ -0,0 +1,558 @@ +// +// Created by nobleo on 12-9-18. +// + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "common.hpp" + +// register planner as move_base and move_base plugins +PLUGINLIB_EXPORT_CLASS( + path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController) + +namespace path_tracking_pid +{ +namespace +{ +constexpr double MAP_PARALLEL_THRESH = 0.2; +constexpr double DT_MAX = 1.5; + +/** + * Convert the plan from geometry message format to tf2 format. + * + * @param[in] plan Plan to convert. + * @return Converted plan. + */ +std::vector convert_plan(const std::vector & plan) +{ + auto result = std::vector{}; + + result.reserve(plan.size()); + std::transform( + plan.cbegin(), plan.cend(), std::back_inserter(result), + [](const geometry_msgs::PoseStamped & msg) { return tf2_convert(msg.pose); }); + + return result; +} + +} // namespace + +void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig & config) +{ + pid_controller_.configure(config); + controller_debug_enabled_ = config.controller_debug_enabled; +} + +void TrackingPidLocalPlanner::initialize( + std::string name, tf2_ros::Buffer * tf, costmap_2d::Costmap2DROS * costmap) +{ + ros::NodeHandle nh("~/" + name); + ros::NodeHandle gn; + ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); + // setup dynamic reconfigure + pid_server_ = + std::make_unique>(config_mutex_, nh); + pid_server_->setCallback( + [this](auto & config, auto /*unused*/) { this->reconfigure_pid(config); }); + pid_controller_.setEnabled(false); + + bool holonomic_robot; + nh.param("holonomic_robot", holonomic_robot, false); + pid_controller_.setHolonomic(holonomic_robot); + + bool estimate_pose_angle; + nh.param("estimate_pose_angle", estimate_pose_angle, false); + pid_controller_.setEstimatePoseAngle(estimate_pose_angle); + + nh.param("base_link_frame", base_link_frame_, "base_link"); + + nh.param("use_tricycle_model", use_tricycle_model_, false); + nh.param("steered_wheel_frame", steered_wheel_frame_, "steer"); + + visualization_ = std::make_unique(nh); + debug_pub_ = nh.advertise("debug", 1); + path_pub_ = nh.advertise("visualization_path", 1, true); + + sub_odom_ = gn.subscribe("odom", 1, &TrackingPidLocalPlanner::curOdomCallback, this); + sub_vel_max_external_ = + nh.subscribe("vel_max", 1, &TrackingPidLocalPlanner::velMaxExternalCallback, this); + feedback_pub_ = nh.advertise("feedback", 1); + + map_frame_ = costmap->getGlobalFrameID(); + costmap_ = costmap; + tf_ = tf; + + initialized_ = true; +} + +bool TrackingPidLocalPlanner::setPlan(const std::vector & global_plan) +{ + if (!initialized_) { + ROS_ERROR( + "path_tracking_pid has not been initialized, please call initialize() before using this " + "planner"); + return false; + } + + auto global_plan_map_frame = global_plan; + + std::string path_frame = global_plan_map_frame.at(0).header.frame_id; + ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan_map_frame.size()); + ROS_DEBUG("Plan is defined in frame '%s'", path_frame.c_str()); + + /* If frame of received plan is not equal to mbf-map_frame, translate first */ + if (map_frame_ != path_frame) { + ROS_DEBUG( + "Transforming plan since my global_frame = '%s' and my plan is in frame: '%s'", + map_frame_.c_str(), path_frame.c_str()); + geometry_msgs::TransformStamped tf_transform; + tf_transform = tf_->lookupTransform(map_frame_, path_frame, ros::Time(0)); + // Check alignment, when path-frame is severly mis-aligned show error + double yaw; + double pitch; + double roll; + tf2::getEulerYPR(tf_transform.transform.rotation, yaw, pitch, roll); + if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) { + ROS_ERROR( + "Path is given in %s frame which is severly mis-aligned with our map-frame: %s", + path_frame.c_str(), map_frame_.c_str()); + } + for (auto & pose_stamped : global_plan_map_frame) { + tf2::doTransform(pose_stamped.pose, pose_stamped.pose, tf_transform); + pose_stamped.header.frame_id = map_frame_; + // 'Project' plan by removing z-component + pose_stamped.pose.position.z = 0.0; + } + } + + if (controller_debug_enabled_) { + nav_msgs::Path received_path; + received_path.header = global_plan_map_frame.at(0).header; + received_path.poses = global_plan_map_frame; + path_pub_.publish(received_path); + } + + try { + ROS_DEBUG( + "map_frame: %s, plan_frame: %s, base_link_frame: %s", map_frame_.c_str(), path_frame.c_str(), + base_link_frame_.c_str()); + tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); + } catch (const tf2::TransformException & ex) { + ROS_ERROR("Received an exception trying to transform: %s", ex.what()); + return false; + } + + // Feasability check, but only when not resuming with odom-vel + if ( + pid_controller_.getConfig().init_vel_method != Pid_Odom && + pid_controller_.getConfig().init_vel_max_diff >= 0.0 && + std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getCurrentForwardVelocity()) > + pid_controller_.getConfig().init_vel_max_diff) { + ROS_ERROR( + "Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", + latest_odom_.twist.twist.linear.x, pid_controller_.getCurrentForwardVelocity()); + return false; + } + + if (use_tricycle_model_) { + try { + ROS_DEBUG( + "base_link_frame: %s, steered_wheel_frame: %s", base_link_frame_.c_str(), + steered_wheel_frame_.c_str()); + tf_base_to_steered_wheel_stamped_ = + tf_->lookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); + } catch (const tf2::TransformException & ex) { + ROS_ERROR("Received an exception trying to transform: %s", ex.what()); + ROS_ERROR( + "Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will " + "be disabled"); + use_tricycle_model_ = false; + } + + pid_controller_.setTricycleModel( + use_tricycle_model_, + tf2_convert(tf_base_to_steered_wheel_stamped_.transform)); + + // TODO(clopez): subscribe to steered wheel odom + geometry_msgs::Twist steering_odom_twist; + if (!pid_controller_.setPlan( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, + tf2_convert(tf_base_to_steered_wheel_stamped_.transform), + steering_odom_twist, convert_plan(global_plan_map_frame))) { + return false; + } + } else { + if (!pid_controller_.setPlan( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, + convert_plan(global_plan_map_frame))) { + return false; + } + } + + pid_controller_.setEnabled(true); + active_goal_ = true; + prev_time_ = ros::Time(0); + return true; +} + +std::optional TrackingPidLocalPlanner::computeVelocityCommands() +{ + ros::Time now = ros::Time::now(); + if (prev_time_.isZero()) { + prev_time_ = now - prev_dt_; // Initialisation round + } + ros::Duration dt = now - prev_time_; + if (dt.isZero()) { + ROS_ERROR_THROTTLE( + 5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); + auto cmd_vel = geometry_msgs::Twist(); + cmd_vel.linear.x = pid_controller_.getCurrentForwardVelocity(); + cmd_vel.angular.z = pid_controller_.getCurrentYawVelocity(); + // At the first call of computeVelocityCommands() we can't calculate a cmd_vel. We can't return + // false because of https://github.com/magazino/move_base_flex/issues/195 so the current + // velocity is send instead. + return cmd_vel; + } + if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { + ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); + return std::nullopt; + } + try { + ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); + tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); + } catch (const tf2::TransformException & ex) { + ROS_ERROR("Received an exception trying to transform: %s", ex.what()); + active_goal_ = false; + return std::nullopt; + } + + // Handle obstacles + if (pid_controller_.getConfig().anti_collision) { + const std::vector footprint = costmap_->getRobotFootprint(); + auto cost = projectedCollisionCost( + costmap_->getCostmap(), footprint, projectionSteps(), visualization_, map_frame_); + + if (cost >= costmap_2d::LETHAL_OBSTACLE) { + pid_controller_.setVelMaxObstacle(0.0); + } else if (pid_controller_.getConfig().obstacle_speed_reduction) { + double max_vel = pid_controller_.getConfig().max_x_vel; + double reduction_factor = static_cast(cost) / costmap_2d::LETHAL_OBSTACLE; + double limit = max_vel * (1 - reduction_factor); + ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit); + pid_controller_.setVelMaxObstacle(limit); + } else { + pid_controller_.setVelMaxObstacle(INFINITY); // set back to inf + } + } else { + pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf + } + + const auto update_result = pid_controller_.update_with_limits( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, dt); + + path_tracking_pid::PidFeedback feedback_msg; + feedback_msg.eda = ros::Duration(update_result.eda); + feedback_msg.progress = update_result.progress; + feedback_pub_.publish(feedback_msg); + + if (cancel_requested_) { + path_tracking_pid::PidConfig config = pid_controller_.getConfig(); + // Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence + // the sign propagates correctly + config.target_x_vel = std::copysign(0.0, config.target_x_vel); + config.target_end_x_vel = std::copysign(0.0, config.target_x_vel); + boost::recursive_mutex::scoped_lock lock(config_mutex_); + // When updating from own server no callback is called. Thus controller is updated first and then server is notified + pid_controller_.configure(config); + pid_server_->updateConfig(config); + lock.unlock(); + cancel_requested_ = false; + } + + if (controller_debug_enabled_) { + debug_pub_.publish(update_result.pid_debug); + + // publish rviz visualization + std_msgs::Header header; + header.stamp = now; + header.frame_id = map_frame_; + const auto tfCurPose = tf2_convert(tfCurPoseStamped_.transform); + visualization_->publishAxlePoint(header, tfCurPose); + visualization_->publishControlPoint(header, pid_controller_.getCurrentWithCarrot()); + visualization_->publishGoalPoint(header, pid_controller_.getCurrentGoal()); + visualization_->publishPlanPoint(header, pid_controller_.getCurrentPosOnPlan()); + } + + prev_time_ = now; + prev_dt_ = + dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) + return update_result.velocity_command; +} + +std::vector TrackingPidLocalPlanner::projectionSteps() +{ + // Check how far we should check forward + double x_vel = pid_controller_.getCurrentForwardVelocity(); + double collision_look_ahead_distance = + x_vel * x_vel / (2 * pid_controller_.getConfig().target_x_decc) + + pid_controller_.getConfig().collision_look_ahead_length_offset; + uint n_steps = std::ceil( + collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); + double x_resolution = collision_look_ahead_distance / std::max(static_cast(n_steps), 1); + + // Define a x_step transform which will be used to step forward the position. + tf2::Transform x_step_tf; + double target_x_vel = pid_controller_.getConfig().target_x_vel; + double max_abs_x_vel = std::abs(x_vel) > std::abs(target_x_vel) ? x_vel : target_x_vel; + x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, max_abs_x_vel), 0.0, 0.0)); + + // Keep track of the projected position on the path. + auto projected_global_plan_index = pid_controller_.getCurrentGlobalPlanIndex(); + + // Step until lookahead is reached, for every step project the pose back to the path + std::vector step_points; + std::vector poses_on_path_points; + std::vector projected_steps_tf; + auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); + projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link + projected_step_tf = + pid_controller_.findPoseOnPlan(projected_step_tf, projected_global_plan_index).pose; + projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose + for (uint step = 0; step < n_steps; step++) { + tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; + projected_step_tf = + pid_controller_.findPoseOnPlan(next_straight_step_tf, projected_global_plan_index).pose; + projected_steps_tf.push_back(projected_step_tf); + + // Fill markers: + step_points.push_back(next_straight_step_tf.getOrigin()); + poses_on_path_points.push_back(projected_step_tf.getOrigin()); + } + + // Visualize + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = map_frame_; + visualization_->publishExtrapolatedPoses(header, step_points); + visualization_->publishgGoalPosesOnPath(header, poses_on_path_points); + + return projected_steps_tf; +} + +boost::geometry::model::ring TrackingPidLocalPlanner::projectionFootprint( + const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame) +{ + std::vector projected_footprint_points; + polygon_t previous_footprint_xy; + polygon_t projected_polygon; + for (const auto & projection_tf : projected_steps) { + // Project footprint forward + double x = projection_tf.getOrigin().x(); + double y = projection_tf.getOrigin().y(); + double yaw = tf2::getYaw(projection_tf.getRotation()); + + // Project footprint forward + std::vector footprint_proj; + costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj); + + // Append footprint to polygon + polygon_t two_footprints = previous_footprint_xy; + previous_footprint_xy.clear(); + for (const auto & point : footprint_proj) { + boost::geometry::append(two_footprints, point); + boost::geometry::append(previous_footprint_xy, point); + } + + boost::geometry::correct(two_footprints); + polygon_t two_footprint_hull; + boost::geometry::convex_hull(two_footprints, two_footprint_hull); + projected_polygon = union_(projected_polygon, two_footprint_hull); + + // Add footprint to marker + geometry_msgs::Point previous_point = footprint_proj.back(); + for (const auto & point : footprint_proj) { + projected_footprint_points.push_back(tf2_convert(previous_point)); + projected_footprint_points.push_back(tf2_convert(point)); + previous_point = point; + } + } + + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = viz_frame; + viz->publishCollisionFootprint(header, projected_footprint_points); + + return projected_polygon; +} + +uint8_t TrackingPidLocalPlanner::projectedCollisionCost( + costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame) +{ + auto collision_polygon = projectionFootprint(footprint, projected_steps, viz, viz_frame); + + // Calculate cost by checking base link location in costmap + uint8_t max_projected_step_cost = 0; + for (const auto & projection_tf : projected_steps) { + int map_x, map_y; + costmap2d->worldToMapEnforceBounds( + projection_tf.getOrigin().x(), projection_tf.getOrigin().y(), map_x, map_y); + uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); + if (projected_step_cost > max_projected_step_cost) { + max_projected_step_cost = projected_step_cost; + } + } + + // Create a convex hull so we can use costmap2d->convexFillCells + polygon_t collision_polygon_hull; + boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); + std::vector collision_polygon_hull_map; + + // Convert to map coordinates + for (const auto & point : collision_polygon_hull) { + int map_x; + int map_y; + costmap2d->worldToMapEnforceBounds(point.x, point.y, map_x, map_y); + costmap_2d::MapLocation map_point{static_cast(map_x), static_cast(map_y)}; + collision_polygon_hull_map.push_back(map_point); + } + + // Get the relevant cells + std::vector cells_in_polygon; + costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon); + + // Get the max cost inside the concave polygon + tf2::Vector3 collision_point; + uint8_t max_cost = 0.0; + for (const auto & cell_in_polygon : cells_in_polygon) { + // Cost checker is cheaper than polygon checker, so lets do that first + uint8_t cell_cost = costmap2d->getCost(cell_in_polygon.x, cell_in_polygon.y); + if (cell_cost > max_cost && cell_cost != costmap_2d::NO_INFORMATION) { + // Check if in concave polygon + geometry_msgs::Point point; + costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y); + if (boost::geometry::within(point, collision_polygon)) { + max_cost = cell_cost; + // Set collision indicator on suspected cell with current cost + collision_point = tf2_convert(point); + if (max_cost >= costmap_2d::LETHAL_OBSTACLE) { + max_projected_step_cost = max_cost; + break; // Collision detected, no need to evaluate further + } + } + } + } + + // Fiddle the polygon into a marker message + std::vector collision_hull_points; + for (const geometry_msgs::Point point : collision_polygon) { + tf2::Vector3 point_tf2; + tf2::fromMsg(point, point_tf2); + collision_hull_points.push_back(point_tf2); + } + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = viz_frame; + viz->publishCollisionObject(header, max_cost, collision_point); + viz->publishCollisionPolygon(header, collision_hull_points); + + return max_projected_step_cost; +} + +uint32_t TrackingPidLocalPlanner::computeVelocityCommands( + const geometry_msgs::PoseStamped & /* pose */, const geometry_msgs::TwistStamped & /* velocity */, + geometry_msgs::TwistStamped & cmd_vel, std::string & /* message */) +{ + if (!initialized_) { + ROS_ERROR( + "path_tracking_pid has not been initialized, please call initialize() before using this " + "planner"); + active_goal_ = false; + return mbf_msgs::ExePathResult::NOT_INITIALIZED; + } + // TODO(Cesar): Use provided pose and odom + const auto opt_cmd_vel = computeVelocityCommands(); + if (!opt_cmd_vel) { + active_goal_ = false; + return mbf_msgs::ExePathResult::FAILURE; + } + cmd_vel.twist = *opt_cmd_vel; + cmd_vel.header.stamp = ros::Time::now(); + cmd_vel.header.frame_id = base_link_frame_; + + bool moving = std::abs(cmd_vel.twist.linear.x) > VELOCITY_EPS; + if (cancel_in_progress_) { + if (!moving) { + ROS_INFO( + "Cancel requested and we now (almost) reached velocity 0: %f", cmd_vel.twist.linear.x); + cancel_in_progress_ = false; + active_goal_ = false; + return mbf_msgs::ExePathResult::CANCELED; + } + ROS_INFO_THROTTLE(1.0, "Cancel in progress... remaining x_vel: %f", cmd_vel.twist.linear.x); + return to_underlying(ComputeVelocityCommandsResult::GRACEFULLY_CANCELLING); + } + + if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) { + active_goal_ = false; + return mbf_msgs::ExePathResult::BLOCKED_PATH; + } + + if (isGoalReached()) { + active_goal_ = false; + } + return mbf_msgs::ExePathResult::SUCCESS; +} + +bool TrackingPidLocalPlanner::isGoalReached() const +{ + // Return reached boolean, but never succeed when we're preempting + return pid_controller_.isEndReached() && !cancel_in_progress_; +} + +bool TrackingPidLocalPlanner::isGoalReached( + double /* dist_tolerance */, double /* angle_tolerance */) +{ + return isGoalReached(); +} + +bool TrackingPidLocalPlanner::cancel() +{ + // This function runs in a separate thread + cancel_requested_ = true; + cancel_in_progress_ = true; + ros::Rate r(10); + ROS_INFO("Cancel requested, waiting in loop for cancel to finish"); + while (active_goal_) { + r.sleep(); + } + ROS_INFO("Finished waiting loop, done cancelling"); + return true; +} + +void TrackingPidLocalPlanner::curOdomCallback(const nav_msgs::Odometry & odom_msg) +{ + latest_odom_ = odom_msg; +} + +void TrackingPidLocalPlanner::velMaxExternalCallback(const std_msgs::Float64 & msg) +{ + pid_controller_.setVelMaxExternal(msg.data); +} +} // namespace path_tracking_pid diff --git a/src/visualization.cpp b/src/visualization.cpp new file mode 100644 index 00000000..3708007c --- /dev/null +++ b/src/visualization.cpp @@ -0,0 +1,184 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace path_tracking_pid +{ +// Factory function for a visualization color from the given arguments. (Because the corresponding +// type has no appropriate constructor.) +std_msgs::ColorRGBA create_color(float r, float g, float b, float a = 1) +{ + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +const auto red = create_color(1, 0, 0); +const auto green = create_color(0, 1, 0); +const auto blue = create_color(1, 0, 1); +const auto yellow = create_color(1, 1, 0); +const auto orange = create_color(1, 0.5, 0); + +std::vector to_msg(std::vector points) +{ + std::vector msgs; + std::transform(points.begin(), points.end(), std::back_inserter(msgs), [](const auto & msg) { + geometry_msgs::Point p; + tf2::toMsg(msg, p); + return p; + }); + return msgs; +} + +Visualization::Visualization(ros::NodeHandle nh) +: marker_pub_{nh.advertise("visualization_marker", 10)} +{ +} + +void Visualization::publishControlPoint( + const std_msgs::Header & header, const tf2::Transform & pose) +{ + publishSphere(header, "control point", pose, green); +} + +void Visualization::publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose) +{ + std_msgs::ColorRGBA color; + publishSphere(header, "axle point", pose, blue); +} + +void Visualization::publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose) +{ + std_msgs::ColorRGBA color; + publishSphere(header, "goal point", pose, red); +} + +void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose) +{ + publishSphere(header, "plan point", pose, orange); +} + +void Visualization::publishCollisionObject( + const std_msgs::Header & header, uint8_t cost, const tf2::Vector3 & point) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "Collision object"; + marker.pose.orientation.w = 1.0; + marker.type = visualization_msgs::Marker::CYLINDER; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.color = red; + marker.color.a = cost / 255.0; + marker.scale.z = cost / 255.0; + tf2::toMsg(point, marker.pose.position); + marker.pose.position.z = marker.scale.z * 0.5; + if (marker.scale.z > std::numeric_limits::epsilon()) { + marker.action = visualization_msgs::Marker::ADD; + } else { + marker.action = visualization_msgs::Marker::DELETE; + } + + marker_pub_.publish(marker); +} + +void Visualization::publishExtrapolatedPoses( + const std_msgs::Header & header, const std::vector & steps) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "extrapolated poses"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.type = visualization_msgs::Marker::POINTS; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.color = orange; + marker.points = to_msg(steps); + marker_pub_.publish(marker); +} + +void Visualization::publishgGoalPosesOnPath( + const std_msgs::Header & header, const std::vector & path) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "goal poses on path"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.type = visualization_msgs::Marker::POINTS; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.color = yellow; + marker.points = to_msg(path); + marker_pub_.publish(marker); +} + +void Visualization::publishCollisionFootprint( + const std_msgs::Header & header, const std::vector & footprint) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "Collision footprint"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.type = visualization_msgs::Marker::LINE_LIST; + marker.scale.x = 0.1; + marker.color = blue; + marker.color.a = 0.3; + marker.points = to_msg(footprint); + marker_pub_.publish(marker); +} + +void Visualization::publishCollisionPolygon( + const std_msgs::Header & header, const std::vector & hull) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "Collision polygon"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.scale.x = 0.2; + marker.color = red; + marker.color.a = 0.3; + marker.points = to_msg(hull); + marker_pub_.publish(marker); +} + +void Visualization::publishSphere( + const std_msgs::Header & header, const std::string & ns, const tf2::Transform & pose, + const std_msgs::ColorRGBA & color) +{ + geometry_msgs::Pose msg; + tf2::toMsg(pose, msg); + publishSphere(header, ns, msg, color); +} + +void Visualization::publishSphere( + const std_msgs::Header & header, const std::string & ns, const geometry_msgs::Pose & pose, + const std_msgs::ColorRGBA & color) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = ns; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose = pose; + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color = color; + marker_pub_.publish(marker); +} + +} // namespace path_tracking_pid diff --git a/test/.gitignore b/test/.gitignore new file mode 100644 index 00000000..c18dd8d8 --- /dev/null +++ b/test/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/test/__init__.py b/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/test/local_costmap_params.yaml b/test/local_costmap_params.yaml new file mode 100644 index 00000000..fb179cf8 --- /dev/null +++ b/test/local_costmap_params.yaml @@ -0,0 +1,41 @@ +local_costmap: + # http://wiki.ros.org/costmap_2d/hydro/obstacles + footprint: [[-6, -2.5], [-6, 2.5], [3, 2], [4.5, 0], [3, -2]] + footprint_padding: 0.1 + transform_tolerance: 1.0 + global_frame: odom + robot_base_frame: base_link + update_frequency: 5.0 + publish_frequency: 5.0 + width: 40.0 + height: 40.0 + resolution: 0.5 # This resolution should be bigger than the spatial resolution of velodyne. Otherwise incorrect costmap clearing occurs + rolling_window: true + + plugins: + - name: obstacles_layer + type: "costmap_2d::ObstacleLayer" + - name: inflation_layer + type: "costmap_2d::InflationLayer" + + obstacles_layer: + enabled: true + footprint_clearing_enabled: false + obstacle_range: 50 + observation_sources: pointcloud + pointcloud: + data_type: PointCloud + topic: /pointcloud + marking: true + clearing: false + + inflation_layer: + enabled: true + inflation_radius: 20 + cost_scaling_factor: 0.1 + +# Disable global costmap: +global_costmap: + plugins: + - name: empty_layer + type: "costmap_2d::InflationLayer" diff --git a/test/paths.py b/test/paths.py new file mode 100644 index 00000000..541aedba --- /dev/null +++ b/test/paths.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python +from copy import deepcopy +from math import cos, pi, sin + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from numpy import linspace +from tf.transformations import quaternion_from_euler + + +def points_on_circle(angles, radius=1, center=(0, 0)): + return [ + ( + center[0] + (sin(angle) * radius), # x + center[1] - (cos(angle) * radius), # y + angle, # yaw + ) + for angle in angles + ] + + +def back_and_forth(radius): + # Construct path to go 10m forward, turn and go back + path = Path() + pose = PoseStamped() + pose.header.frame_id = "map" + pose.pose.orientation.w = 1.0 + pose.pose.position.x = 0.0 + path.poses.append(deepcopy(pose)) + + for (x, y, yaw) in points_on_circle(linspace(0, pi), radius=radius, center=(10, radius)): + pose.pose.position.x = x + pose.pose.position.y = y + quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = quat[0] + pose.pose.orientation.y = quat[1] + pose.pose.orientation.z = quat[2] + pose.pose.orientation.w = quat[3] + path.poses.append(deepcopy(pose)) + + pose.pose.position.x = 0 + path.poses.append(deepcopy(pose)) + return path + + +def reverse_path(path): + path.poses.reverse() + return path + + +def create_path(x, y, yaw): + # Construct a path based on three vectors + path = Path() + pose = PoseStamped() + pose.header.frame_id = "map" + + for (x, y, yaw) in zip(x, y, yaw): + pose.pose.position.x = x + pose.pose.position.y = y + quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = quat[0] + pose.pose.orientation.y = quat[1] + pose.pose.orientation.z = quat[2] + pose.pose.orientation.w = quat[3] + path.poses.append(deepcopy(pose)) + + return path diff --git a/test/test.rviz b/test/test.rviz new file mode 100644 index 00000000..562854eb --- /dev/null +++ b/test/test.rviz @@ -0,0 +1,238 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 316 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: /move_base_flex/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: /move_base_flex/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.05000000074505806 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.05000000074505806 + Topic: /move_base_flex/PathTrackingPID/visualization_path + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.019999999552965164 + Head Radius: 0.05000000074505806 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.019999999552965164 + Value: Arrow + Topic: /odom + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Topic: /move_base_flex/global_costmap/footprint + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /move_base_flex/PathTrackingPID/visualization_marker + Name: Marker + Namespaces: + axle point: true + control point: true + goal point: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /move_base_flex/PathTrackingPID/collision_markers + Name: MarkerArray + Namespaces: + Collision footprint: true + Collision polygon: true + extrapolated poses: true + goal poses on path: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /set_pose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -1.5700007677078247 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 20.735715866088867 + Target Frame: + Value: TopDownOrtho (rviz) + X: 7.837769985198975 + Y: 3.2284839153289795 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 738 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000023ffc0200000008fb0000001200530065006c0065006300740069006f006e01000000410000006e0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ec000001c200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000b5000001cb000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000023f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: true + Time: + collapsed: true + Tool Properties: + collapsed: true + Views: + collapsed: true + Width: 927 + X: 309 + Y: 143 diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py new file mode 100755 index 00000000..74ec6f68 --- /dev/null +++ b/test/test_path_tracking_pid.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +import unittest + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from geometry_msgs.msg import Point32, PoseWithCovarianceStamped, Twist +from mbf_msgs.msg import ExePathAction, ExePathGoal +from path_tracking_pid.msg import PidDebug +from sensor_msgs.msg import PointCloud +from std_msgs.msg import Float64 +from tf.transformations import quaternion_from_euler + +from paths import back_and_forth, reverse_path + + +class SlowDownChecker(object): + def __init__(self, target_vel): + self.target_vel = target_vel + self.slowed_down = False + self.kept_slow = True # Flag to see if we did not accidentaly speed up again + self.vel = 0.0 + rospy.Subscriber("cmd_vel", Twist, self.cb) + + def cb(self, msg): + decelerating = msg.linear.x < self.vel + # if msg.linear.x < self.vel: + self.vel = msg.linear.x # deceleration phase + if not self.slowed_down and decelerating and self.vel < self.target_vel + 0.01: + rospy.loginfo("Slowed down to {}m/s".format(self.target_vel)) + self.slowed_down = True + if self.slowed_down and self.kept_slow and self.vel > self.target_vel + 0.1: + rospy.logwarn("Sped up again while limit is still active") + self.kept_slow = False + +class ErrorCatcher(object): + def __init__(self): + self.error = Twist() + rospy.Subscriber("move_base_flex/PathTrackingPID/debug", PidDebug, self.cb) + + def cb(self, msg): + self.error = msg.tracking_error + +class TestPathTrackingPID(unittest.TestCase): + def test_exepath_action(self): + # Set inital pose to parameter + if rospy.has_param("~initialpose"): + values = rospy.get_param("~initialpose") + rospy.loginfo("Initial pose to {}".format(values)) + pose = PoseWithCovarianceStamped() + pose.header.frame_id = "map" + pose.pose.pose.position.x = values[0] + pose.pose.pose.position.y = values[1] + quat = quaternion_from_euler(0, 0, values[2]) + pose.pose.pose.orientation.x = quat[0] + pose.pose.pose.orientation.y = quat[1] + pose.pose.pose.orientation.z = quat[2] + pose.pose.pose.orientation.w = quat[3] + initialpose_pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, latch=True, queue_size=1) + initialpose_pub.publish(pose) + rospy.sleep(0.1) # Fill tf buffers + + self.max_tracking_error_linear_x = rospy.get_param("~max_tracking_error_linear_x", 0.1) + self.max_tracking_error_linear_y = rospy.get_param("~max_tracking_error_linear_y", 0.1) + self.max_tracking_error_angular_z = rospy.get_param("~max_tracking_error_angular_z", 0.1) + + # Publisher for obstacles: + self.obstacle_pub = rospy.Publisher("pointcloud", PointCloud, latch=True, queue_size=1) + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Spawn desired obstacles: + pc = PointCloud() + pc.header.stamp = rospy.Time.now() + pc.header.frame_id = "map" + for point in rospy.get_param("~obstacles", []): + pc.points.append(Point32(x=point[0], y=point[1])) + self.obstacle_pub.publish(pc) + + path = back_and_forth(rospy.get_param("~radius", 5.0)) + + # Listen to errors: + error_catcher = ErrorCatcher() + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + client.send_goal(ExePathGoal(path=path)) + + max_vel = rospy.get_param("~max_vel", 0.0) + if max_vel > 0.0: + reconfigure.update_configuration({"target_end_x_vel": 5.0}) + checker = SlowDownChecker(max_vel) + max_vel_pub = rospy.Publisher("move_base_flex/PathTrackingPID/vel_max", Float64, queue_size=1) + rospy.sleep(3.0) # Accelerate first + max_vel_pub.publish(max_vel) + rospy.sleep(10.0) + self.assertTrue(checker.slowed_down) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(120)) + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + + if max_vel > 0.0: + self.assertTrue(checker.kept_slow) + return + + # Check the errors + self.assertLess(error_catcher.error.linear.x, self.max_tracking_error_linear_x) + self.assertLess(error_catcher.error.linear.y, self.max_tracking_error_linear_y) + self.assertLess(error_catcher.error.angular.z, self.max_tracking_error_angular_z) + + # Do the same for backward movements if last path was a success + if client.get_state() != GS.SUCCEEDED or rospy.get_param("backward", True): + return + + reconfigure.update_configuration({"target_x_vel": -5.0}) + client.send_goal(ExePathGoal(path=reverse_path(path))) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid", anonymous=False) + rostest.rosrun("back_and_forth", "rostest_path_tracking_pid", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test new file mode 100644 index 00000000..46a14c88 --- /dev/null +++ b/test/test_path_tracking_pid.test @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + [0, 2, 0] + False + + + [0, 2, 3] + False + + + [0, 0, 1.57] + False + + + [20, 0, 0] + False + + + + [3, -25, 0] + False + + + + [[10.0, 10.0]] + 4 + 1.0 + + + [[15.0, -8.0]] + + + 1.0 + true + + + 1.0 + + + + + + + + diff --git a/test/test_path_tracking_pid_accel.py b/test/test_path_tracking_pid_accel.py new file mode 100755 index 00000000..e58ef1c6 --- /dev/null +++ b/test/test_path_tracking_pid_accel.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python +import unittest +from math import hypot + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry + +from paths import create_path + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def reconfigure(self): + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": 2.0}) + reconfigure.update_configuration({"target_end_x_vel": 0}) + reconfigure.update_configuration({"target_x_acc": 1.0}) + reconfigure.update_configuration({"target_x_decc": 1.0}) + reconfigure.update_configuration({"use_mpc": False}) + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path = create_path([0.0, 10.0], [0.0, 0.0], [0.0, 0.0]) + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure() + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + # Preempt action after 2s + rospy.sleep(2.0) + client.cancel_goal() + rospy.sleep(1.0) + self.assertTrue(1.0 < abs(self.cur_odom.twist.twist.linear.x) < 2.0, msg="Violated deceleration on preempt") + rospy.sleep(2.0) + self.assertTrue(abs(self.cur_odom.twist.twist.linear.x) < 0.1, msg="Violated deceleration on preempt") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + self.assertEqual(client.get_state(), GS.PREEMPTED, msg="Action didn't preempt on request: {}".format(client.get_state())) + + # Resume action + self.reconfigure() + client.send_goal(ExePathGoal(path=path)) + rospy.sleep(1.0) + self.assertEqual(client.get_state(), GS.ACTIVE, msg="Action didn't restart on request") + self.assertTrue(0.1 < abs(self.cur_odom.twist.twist.linear.x) < 2.0, msg="Violated acceleration on restart") + rospy.sleep(1.0) + self.assertTrue(abs(self.cur_odom.twist.twist.linear.x) > 1.9, msg="Violated acceleration on restart") + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_accel", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_accel", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid_bw_turn_cancel.py b/test/test_path_tracking_pid_bw_turn_cancel.py new file mode 100755 index 00000000..5f799f72 --- /dev/null +++ b/test/test_path_tracking_pid_bw_turn_cancel.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +import unittest +from math import cos, hypot, pi, sin + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from geometry_msgs.msg import Point, Pose +from math import atan2 +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry +from numpy import linspace +from visualization_msgs.msg import Marker + +from paths import create_path + + +def points_on_circle(angles, radius=1, center=(0, 0)): + x = [] + y = [] + yaw = [] + for angle in angles: + x.append(center[0] + (cos(angle) * radius)) + y.append(center[1] + (sin(angle) * radius)) + yaw.append(angle + pi*0.5) + return (x, y, yaw) + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def vis_cb(self, msg): + if msg.ns == 'control point': + self.carrot = msg.pose.position + elif msg.ns == 'plan point': + self.pos_on_plan = msg.pose.position + + # Only start checking when both markers are received + if self.carrot is None or self.pos_on_plan is None: + return + + angle = atan2(self.carrot.y - self.pos_on_plan.y, + self.carrot.x - self.pos_on_plan.x) + # Check if marker direction doesn't flip, if it did, hold this boolean + if not self.carrot_dir_flipped and self.marker_angle is not None: + angle_diff = angle - self.marker_angle + # 'Wrap' + if angle_diff > pi: + angle_diff -= 2*pi + elif angle_diff < -pi: + angle_diff += 2*pi + # Check if angle flipped + self.carrot_dir_flipped = abs(angle_diff) > 0.8*pi + self.marker_angle = angle + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + self.carrot = None + self.pos_on_plan = None + self.marker_angle = None + self.carrot_dir_flipped = None + # Subscribe to visualization-markers to track control direction + self.vis_sub = rospy.Subscriber("move_base_flex/PathTrackingPID/visualization_marker", Marker, self.vis_cb) + + (x_circ, y_circ, yaw_circ) = points_on_circle(linspace(-0.5*pi, -0.75*pi), radius=3.0, center=(0.0, 3.0)) + path = create_path(x_circ, y_circ, yaw_circ) + + speed = -0.6 + endspeed = 0 + + # Start goal and cancel in between + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": speed}) + reconfigure.update_configuration({"target_end_x_vel": endspeed}) + reconfigure.update_configuration({"target_x_acc": 0.5}) + reconfigure.update_configuration({"target_x_decc": 0.5}) + reconfigure.update_configuration({"use_mpc": False}) + # We require debug enabled here to retrieve the markers! + reconfigure.update_configuration({"controller_debug_enabled": True}) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(2.0) + rospy.logwarn("Preempting path!") + client.cancel_goal() + rospy.logwarn("Wait for result") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + rospy.logwarn("Got result") + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + + self.assertTrue(self.carrot_dir_flipped is False, msg="Guiding direction flipped while stopping!") + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_bw_turn_cancel", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_bw_turn_cancel", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid_fw_bw.py b/test/test_path_tracking_pid_fw_bw.py new file mode 100755 index 00000000..ada885b3 --- /dev/null +++ b/test/test_path_tracking_pid_fw_bw.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +import unittest +from math import hypot + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry + +from paths import create_path + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + self.prev_odom = Odometry() + self.cur_accel = 0.0 + + def reconfigure(self, target_vel): + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": target_vel}) + reconfigure.update_configuration({"target_end_x_vel": 0}) + reconfigure.update_configuration({"target_x_acc": 4.0}) + reconfigure.update_configuration({"target_x_decc": 1.0}) + reconfigure.update_configuration({"use_mpc": False}) + + def odom_cb(self, msg): + self.prev_odom = self.cur_odom + self.cur_odom = msg + dv = self.cur_odom.twist.twist.linear.x - self.prev_odom.twist.twist.linear.x + dt = (self.cur_odom.header.stamp - self.prev_odom.header.stamp).to_sec() + self.cur_accel = dv / dt + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path = create_path([0.0, 10.0], [0.0, 0.0], [0.0, 0.0]) + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure(2.0) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(0.5) + self.assertTrue(3.5 < self.cur_accel < 4.5, msg="Violated acceleration {}".format(self.cur_accel)) + rospy.sleep(4.5) + self.assertTrue(-1.5 < self.cur_accel < -0.5, msg="Violated deceleration {}".format(self.cur_accel)) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + # Start bw-goal and evaluate outcome + path = create_path([10.0, 0.0], [0.0, 0.0], [0.0, 0.0]) + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure(-2.0) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(0.5) + self.assertTrue(-4.5 < self.cur_accel < -3.5, msg="Violated acceleration {}".format(self.cur_accel)) + rospy.sleep(4.5) + self.assertTrue(0.5 < self.cur_accel < 1.5, msg="Violated deceleration {}".format(self.cur_accel)) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_fw_bw", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_fw_bw", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid_preempt.py b/test/test_path_tracking_pid_preempt.py new file mode 100755 index 00000000..7a46b7ce --- /dev/null +++ b/test/test_path_tracking_pid_preempt.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python +import unittest +from math import hypot + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry + +from paths import create_path + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def reconfigure(self, end_vel=0.0): + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": 2.0}) + reconfigure.update_configuration({"target_end_x_vel": end_vel}) + reconfigure.update_configuration({"target_x_acc": 2.0}) + reconfigure.update_configuration({"target_x_decc": 1.0}) + reconfigure.update_configuration({"use_mpc": False}) + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path = create_path([0.0, 2.0], [0.0, 0.0], [0.0, 0.0]) + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure(end_vel = 1.0) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + # Preempt action after 1.5s (in deceleration phase) + rospy.sleep(1.5) + rospy.logwarn("Preempting path!") + client.cancel_goal() + rospy.logwarn("Wait for result") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + rospy.logwarn("Got result") + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + # self.assertEqual(client.get_state(), GS.ABORTED, msg="Action didn't preempt on request") + + # # Resume action + rospy.logwarn("Reconfiguring for resume") + self.reconfigure(end_vel = 0.0) + rospy.logwarn("Creating resume path") + path = create_path([2.0, 4.0], [0.0, 0.0], [0.0, 0.0]) + rospy.logwarn("Sending new goal") + client.send_goal(ExePathGoal(path=path)) + rospy.logwarn("Goal sent!") + rospy.sleep(1.0) + rospy.logwarn("Waiting for new result") + self.assertEqual(client.get_state(), GS.ACTIVE, msg="Action didn't restart on request") + rospy.logwarn("FINISHED!") + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(5)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_preempt", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_preempt", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid_turn_skip.py b/test/test_path_tracking_pid_turn_skip.py new file mode 100755 index 00000000..2355f5c9 --- /dev/null +++ b/test/test_path_tracking_pid_turn_skip.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python +import unittest +from math import cos, hypot, pi, sin + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry +from numpy import linspace + +from paths import create_path + + +def points_on_circle(angles, radius=1, center=(0, 0)): + x = [] + y = [] + yaw = [] + for angle in angles: + x.append(center[0] + (cos(angle) * radius)) + y.append(center[1] + (sin(angle) * radius)) + yaw.append(angle + pi*0.5) + return (x, y, yaw) + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path0a = create_path([0.0, 3.5], [0.0, 0.0], [0.0, 0.0]) + path0b = create_path([3.5, 4.5], [0.0, 0.0], [0.0, 0.0]) + path0c = create_path([4.5, 5.0], [0.0, 0.0], [0.0, 0.0]) + (x_circ, y_circ, yaw_circ) = points_on_circle(linspace(-0.5*pi, -0.75*pi), radius=2.0, center=(5.0, 2.0)) + path1 = create_path(x_circ, y_circ, yaw_circ) + path2 = create_path([cos(-0.75*pi)*2.0+5.0, cos(-0.75*pi)*2.0+10.0], + [sin(-0.75*pi)*2.0+2.0, sin(-0.75*pi)*2.0-3.0], + [-0.25*pi, -0.25*pi]) + + paths = [path0a, path0b, path0c, path1, path2] + speeds = [1.2, 1.7, 0.9, -0.6, 1.2] + endspeeds = [0.0, 0.833, 0.0, 0.0, 1.66] + + for (path, speed, endspeed) in zip(paths, speeds, endspeeds): + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": speed}) + reconfigure.update_configuration({"target_end_x_vel": endspeed}) + reconfigure.update_configuration({"use_mpc": False}) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 1.0, msg= + "Did not arrive on final path's pose! pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_turn_skip", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_turn_skip", TestPathTrackingPID) diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp new file mode 100644 index 00000000..c845da7f --- /dev/null +++ b/test/unittests/test_calculations.cpp @@ -0,0 +1,322 @@ +#include +#include + +#include +#include + +#include "../../src/calculations.hpp" + +namespace +{ +using path_tracking_pid::closestPoseOnSegment; +using path_tracking_pid::deltas_of_plan; +using path_tracking_pid::distances_to_goal; +using path_tracking_pid::distSquared; +using path_tracking_pid::getControlPointPose; +using path_tracking_pid::inverse_turning_radiuses; +using path_tracking_pid::is_in_direction_of_target; + +constexpr auto eps = 1e-6; + +// Create a transform (with an identity basis) based on the given coordinates. +tf2::Transform create_transform(double x, double y, double z) +{ + tf2::Transform result; + result.getBasis().setIdentity(); + result.setOrigin({x, y, z}); + return result; +} + +// Create a quaternion based on the given roll, pitch and yaw. +tf2::Quaternion create_quaternion(double roll, double pitch, double yaw) +{ + tf2::Quaternion result; + result.setRPY(roll, pitch, yaw); + return result; +} + +TEST(PathTrackingPidCalculations, DeltasOfPlan_Empty) +{ + const auto plan = std::vector{}; + const auto result = deltas_of_plan(plan); + + EXPECT_TRUE(result.empty()); +} + +TEST(PathTrackingPidCalculations, DeltasOfPlan_OnlyOne) +{ + const auto plan = std::vector{create_transform(1, 1, 1)}; + const auto result = deltas_of_plan(plan); + + EXPECT_TRUE(result.empty()); +} + +TEST(PathTrackingPidCalculations, DeltasOfPlan) +{ + const auto plan = std::vector{ + create_transform(0, 0, 0), create_transform(1, 1, 1), create_transform(0, 0, 4)}; + const auto ref = + std::vector{create_transform(1, 1, 1), create_transform(-1, -1, 3)}; + const auto result = deltas_of_plan(plan); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal_Empty) +{ + const auto deltas = std::vector{}; + const auto ref = std::vector{0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal_Single) +{ + const auto deltas = std::vector{create_transform(3, 4, 0)}; + const auto ref = std::vector{5.0, 0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 0), create_transform(-2, 0, 0), create_transform(0, 1, 0)}; + const auto ref = std::vector{8.0, 3.0, 1.0, 0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal_IgnoreZ) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 1), create_transform(-2, 0, 2), create_transform(0, 1, 3)}; + const auto ref = std::vector{8.0, 3.0, 1.0, 0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_Empty) +{ + const auto deltas = std::vector{}; + const auto ref = std::vector{0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_SingleTooSmall) +{ + const auto deltas = std::vector{create_transform(0, 0, 0)}; + const auto ref = std::vector{std::numeric_limits::infinity(), 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_Single) +{ + const auto deltas = std::vector{create_transform(3, 4, 0)}; + const auto ref = std::vector{8.0 / 25.0, 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 0), create_transform(2, 0, 0), create_transform(0, 2, 0)}; + const auto ref = std::vector{8.0 / 25.0, 0.0, 1.0, 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_IgnoreZ) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 1), create_transform(2, 0, 2), create_transform(0, 2, 3)}; + const auto ref = std::vector{8.0 / 25.0, 0.0, 1.0, 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +// Parameters for parameterized tests of is_in_direction_of_target(). +struct IsInDirectionOfTargetTestParams +{ + bool result = false; + tf2::Transform current; + tf2::Vector3 target; + double velocity = 0.0; +}; + +// Fixture for tests of is_in_direction_of_target(). +class IsInDirectionOfTargetTestFixture +: public ::testing::TestWithParam +{ +}; + +TEST_P(IsInDirectionOfTargetTestFixture, IsInDirectionOfTarget) +{ + const auto & [result, current, target, velocity] = GetParam(); + + EXPECT_EQ(result, is_in_direction_of_target(current, target, velocity)); + EXPECT_EQ(!result, is_in_direction_of_target(current, target, -velocity)); +} + +// Input for parameterized tests of is_in_direction_of_target(). +static const auto is_in_direction_of_target_params = std::vector({ + // base tests: x delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 3, 4}}, {3, 3, 4}, 1.0}, + // 0 velocity + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 3, 4}}, {3, 3, 4}, 0.0}, + // already at target + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {3, 3, 4}}, {3, 3, 4}, 1.0}, + // past target + {false, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {4, 3, 4}}, {3, 3, 4}, 1.0}, + // y delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {3, 2, 4}}, {3, 3, 4}, 1.0}, + // z delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {3, 3, 3}}, {3, 3, 4}, 1.0}, + // xyz delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 2, 3}}, {3, 3, 4}, 1.0}, + // 0 basis + {true, tf2::Transform{{0, 0, 0, 0, 0, 0, 0, 0, 0}, {2, 3, 4}}, {3, 4, 5}, 1.0}, + // inspired by actual values from integration tests + {false, tf2::Transform{{-1, -0.5, 0, 0.5, -1, 0, 0, 0, 1}, {-1, 2, 0}}, {-0.5, 2, 0}, 0.5}, + {false, tf2::Transform{{-1, 0.5, 0, -0.5, -1, 0, 0, 0, 1}, {-0.6, 2, 0}}, {-0.5, 2, 0}, 0.5}, + {true, tf2::Transform{{-0.5, -1, 0, 1, -0.5, 0, 0, 0, 1}, {12, 1.5, 0}}, {11, 1.8, 0}, 0.5}, + {true, tf2::Transform{{-1, -0.5, 0, 0.5, -1, 0, 0, 0, 1}, {11, 2.3, 0}}, {10, 2.1, 0}, 0.7}, + {true, tf2::Transform{{-1, 0.5, 0, -0.5, -1, 0, 0, 0, 1}, {9, 2.2, 0}}, {8, 2, 0}, 0.5}, + {true, tf2::Transform{{0.1, -1, 0, 1, 0.1, 0, 0, 0, 1}, {12, 1, 0}}, {11, 2, 0}, 0.5}, + {true, tf2::Transform{{1, -0.5, 0, 0.5, 1, 0, 0, 0, 1}, {11.2, 0.1, 0}}, {11.1, 0.7, 0}, 0.5}, +}); + +INSTANTIATE_TEST_CASE_P( + PathTrackingPidCalculations, IsInDirectionOfTargetTestFixture, + ::testing::ValuesIn(is_in_direction_of_target_params), + [](const auto & info) { return std::to_string(info.index); }); + +TEST(PathTrackingPidCalculations, DistSquared) +{ + EXPECT_EQ(14, distSquared(create_transform(1, 2, 3), create_transform(2, 4, 6))); + EXPECT_EQ(45, distSquared(create_transform(6, 2, 0), create_transform(2, 4, -5))); + EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3))); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_AtEnd) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = end; + const auto ref = end; + + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_AtStart) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = start; + const auto ref = start; + + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_CloseToEnd) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(7, 5, 0); + const auto ref = end; + + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_CloseToStart) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(-7, -5, 0); + const auto ref = start; + + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_Halfway) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = create_transform(3, 3, 0); + + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_TwoThirds) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(8, 5, 0); + const auto point = create_transform(2, 12, 0); + const auto ref = create_transform(6, 4, 0); + + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_OtherYaw) +{ + const auto start = tf2::Transform(create_quaternion(1, 1, 1), {2, 2, 0}); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = tf2::Transform(create_quaternion(0, 0, 1), {3, 3, 0}); + const auto result = closestPoseOnSegment(point, start, end, false); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], 1e-6); + } + } +} + +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_EstimateAngle) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = tf2::Transform(create_quaternion(0, 0, M_PI / 4.0), {3, 3, 0}); + const auto result = closestPoseOnSegment(point, start, end, true); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], 1e-6); + } + } +} + +TEST(PathTrackingPidCalculations, getControlPointPose) +{ + auto pose = tf2::Transform{create_quaternion(0, 0, M_PI_4), tf2::Vector3{2, 1, 0}}; + auto result = getControlPointPose(pose, M_SQRT2); + auto ref = tf2::Transform(create_quaternion(0, 0, M_PI_4), {3, 2, 0}); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], eps); + } + } +} + +} // namespace diff --git a/test/unittests/test_derivative.cpp b/test/unittests/test_derivative.cpp new file mode 100644 index 00000000..23f033e5 --- /dev/null +++ b/test/unittests/test_derivative.cpp @@ -0,0 +1,34 @@ +#include + +#include +#include + +using path_tracking_pid::details::Derivative; + +constexpr double eps = 1e-6; + +TEST(Derivative, StepResponse) +{ + double dt = 0.1; + + Derivative filter; + + std::vector expected_response = {10, 0, 0}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(Derivative, Reset) +{ + double dt = 0.1; + + Derivative filter; + + EXPECT_NEAR(filter.filter(1, dt), 10, eps); + filter.reset(); + EXPECT_NEAR(filter.filter(0, dt), 0, eps); +} diff --git a/test/unittests/test_fifo_array.cpp b/test/unittests/test_fifo_array.cpp new file mode 100644 index 00000000..bbf8cfdb --- /dev/null +++ b/test/unittests/test_fifo_array.cpp @@ -0,0 +1,126 @@ +#include + +#include + +namespace +{ +using path_tracking_pid::details::FifoArray; + +TEST(PathTrackingPidDetailsFifoArray, Initialize) +{ + FifoArray fifo; + + EXPECT_EQ(fifo[0], 0); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +TEST(PathTrackingPidDetailsFifoArray, Push) +{ + FifoArray fifo; + + fifo.push(42); + + EXPECT_EQ(fifo[0], 42); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +TEST(PathTrackingPidDetailsFifoArray, Push_UntilFull) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + + EXPECT_EQ(fifo[0], -4); + EXPECT_EQ(fifo[1], 111); + EXPECT_EQ(fifo[2], 42); +} + +TEST(PathTrackingPidDetailsFifoArray, Push_BeyondFull) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + fifo.push(314); + + EXPECT_EQ(fifo[0], 314); + EXPECT_EQ(fifo[1], -4); + EXPECT_EQ(fifo[2], 111); +} + +TEST(PathTrackingPidDetailsFifoArray, Reset) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + fifo.push(314); + + fifo.reset(); + + EXPECT_EQ(fifo[0], 0); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +TEST(PathTrackingPidDetailsFifoArray, At) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + + EXPECT_EQ(fifo[0], fifo.at<0>()); + EXPECT_EQ(fifo[1], fifo.at<1>()); + EXPECT_EQ(fifo[2], fifo.at<2>()); +} + +TEST(PathTrackingPidDetailsFifoArray, OtherType) +{ + FifoArray fifo; + + fifo.push(24.0); + fifo.push(0.5); + fifo.push(0.25); + + EXPECT_EQ(fifo[0], 0.25); + EXPECT_EQ(fifo[1], 0.5); + EXPECT_EQ(fifo[2], 24.0); +} + +TEST(PathTrackingPidDetailsFifoArray, OtherSize) +{ + FifoArray fifo; + + fifo.push(1); + fifo.push(2); + fifo.push(3); + fifo.push(4); + fifo.push(5); + fifo.push(6); + + EXPECT_EQ(fifo[0], 6); + EXPECT_EQ(fifo[1], 5); + EXPECT_EQ(fifo[2], 4); + EXPECT_EQ(fifo[3], 3); + EXPECT_EQ(fifo[4], 2); +} + +TEST(PathTrackingPidDetailsFifoArray, Assign) +{ + FifoArray fifo; + fifo[0] = 1; + + EXPECT_EQ(fifo[0], 1); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +} // namespace diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp new file mode 100644 index 00000000..1032cfc9 --- /dev/null +++ b/test/unittests/test_integral.cpp @@ -0,0 +1,53 @@ +#include + +#include +#include + +using path_tracking_pid::details::Integral; + +constexpr double eps = 1e-6; + +TEST(Integral, StepResponse) +{ + double dt = 0.1; + double windup_limit = 0.5; + + Integral filter{windup_limit}; + + std::vector expected_response = {0.05, 0.15, 0.25, 0.35, 0.45, 0.5, 0.5}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(Integral, Reset) +{ + double dt = 0.1; + double windup_limit = 0.5; + + Integral filter{windup_limit}; + + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); + filter.reset(); + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); +} + +TEST(Integral, Configure) +{ + double dt = 0.1; + double windup_limit = 0.2; + + Integral filter{windup_limit}; + + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.20, eps); + filter.configure(0.35); + EXPECT_NEAR(filter.filter(1, dt), 0.30, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.35, eps); +} diff --git a/test/unittests/test_main.cpp b/test/unittests/test_main.cpp new file mode 100644 index 00000000..01836b2b --- /dev/null +++ b/test/unittests/test_main.cpp @@ -0,0 +1,7 @@ +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/unittests/test_second_order_lowpass.cpp b/test/unittests/test_second_order_lowpass.cpp new file mode 100644 index 00000000..93d23ea8 --- /dev/null +++ b/test/unittests/test_second_order_lowpass.cpp @@ -0,0 +1,133 @@ +#include + +#include +#include +#include + +using path_tracking_pid::details::SecondOrderLowpass; + +constexpr double eps = 1e-6; + +TEST(SecondOrderLowpass, StepResponse) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488, + 0.968659, 0.984211, 0.991911, 0.995898, 0.997907}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Disable) +{ + double dt = 0.1; + double cutoff = 0; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + EXPECT_NEAR(filter.filter(0, dt), 0, eps); + EXPECT_NEAR(filter.filter(1, dt), 1, eps); + EXPECT_NEAR(filter.filter(5, dt), 5, eps); +} + +TEST(SecondOrderLowpass, StepResponseCutoff) +{ + double dt = 0.1; + double cutoff = 1 / dt / 8; // lower cutoff so slower response + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = { + 0.068087, 0.255112, 0.461572, 0.612177, 0.720691, + 0.798844, 0.855129, 0.895665, 0.924859, 0.945884, + }; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, StepResponseDamping) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2) * 2; // more damping + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = { + 0.101795, 0.318258, 0.494899, 0.618187, 0.716157, + 0.786043, 0.84057, 0.880057, 0.91048, 0.932743, + }; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Reset) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } + + filter.reset(); + + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Configure) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } + + filter.configure(cutoff / 2, damping); + + // no configure step response is {0.968659, 0.984211, 0.991911, 0.995898, 0.997907} + expected_response = {0.957154, 0.969162, 0.977792, 0.984006, 0.988481}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} diff --git a/trajectories/back_and_forth.yaml b/trajectories/back_and_forth.yaml new file mode 100644 index 00000000..33e8b31f --- /dev/null +++ b/trajectories/back_and_forth.yaml @@ -0,0 +1,228 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/coverage.yaml b/trajectories/coverage.yaml new file mode 100644 index 00000000..e2733dff --- /dev/null +++ b/trajectories/coverage.yaml @@ -0,0 +1,228 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/coverage_actiongoal.yaml b/trajectories/coverage_actiongoal.yaml new file mode 100644 index 00000000..dffb807a --- /dev/null +++ b/trajectories/coverage_actiongoal.yaml @@ -0,0 +1,241 @@ +header: + seq: 1 + stamp: + secs: 1545129449 + nsecs: 998500108 + frame_id: '' +goal_id: + stamp: + secs: 1545129449 + nsecs: 998473882 + id: "/axclient_15430_1545129402991-1-1545129449.998" +goal: + path: + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/trajectories/coverage_map.yaml b/trajectories/coverage_map.yaml new file mode 100644 index 00000000..8a94c98b --- /dev/null +++ b/trajectories/coverage_map.yaml @@ -0,0 +1,228 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/double_points.yaml b/trajectories/double_points.yaml new file mode 100644 index 00000000..706d0538 --- /dev/null +++ b/trajectories/double_points.yaml @@ -0,0 +1,245 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 9 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 13 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/empty.yaml b/trajectories/empty.yaml new file mode 100644 index 00000000..d018af4a --- /dev/null +++ b/trajectories/empty.yaml @@ -0,0 +1,7 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: [] \ No newline at end of file diff --git a/trajectories/empty_frame_ids.yaml b/trajectories/empty_frame_ids.yaml new file mode 100644 index 00000000..89cecb8e --- /dev/null +++ b/trajectories/empty_frame_ids.yaml @@ -0,0 +1,92 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + pose: + position: + x: 0.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/trajectories/test_path.yaml b/trajectories/test_path.yaml new file mode 100644 index 00000000..5a1f7496 --- /dev/null +++ b/trajectories/test_path.yaml @@ -0,0 +1,92 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/zigzag.yaml b/trajectories/zigzag.yaml new file mode 100644 index 00000000..c57533be --- /dev/null +++ b/trajectories/zigzag.yaml @@ -0,0 +1,143 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file