diff --git a/.gitignore b/.gitignore new file mode 100755 index 00000000..a1fb10cc --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.pb.cc +*.pb.h diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 00000000..eb7d415e --- /dev/null +++ b/.travis.yml @@ -0,0 +1,14 @@ +sudo: required + +language: + - generic + +service: + - docker + +before_install: + - docker pull kevinlad/ros-nav-ubuntu-64 + +install: + - docker run -dit --name rrts_test kevinlad/ros-nav-ubuntu-64 + - docker exec rrts_test bash rrts_build_test.sh diff --git a/COPYING.txt b/COPYING.txt new file mode 100644 index 00000000..b2736505 --- /dev/null +++ b/COPYING.txt @@ -0,0 +1,696 @@ +RoboRTS is an open source software stack for Real-Time Strategy research +on mobile robots. + +Copyright (C) 2019 RoboMaster. + +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 . + +The full text of the GPLv3 license follows below. + +-------------------------------------------------------------------------- + + 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) 2018 RoboMaster. + 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 +. diff --git a/README.md b/README.md new file mode 100644 index 00000000..72f9acca --- /dev/null +++ b/README.md @@ -0,0 +1,35 @@ +# RoboRTS +[![Build Status](https://travis-ci.org/RoboMaster/RoboRTS.svg?branch=master)](https://travis-ci.org/RoboMaster/RoboRTS) +[![Gitter](https://badges.gitter.im/RoboMaster/RoboRTS.svg)](https://gitter.im/RoboMaster/RoboRTS?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge) + +## Introduction + + + +RoboRTS is an open source software stack for Real-Time Strategy research on mobile robots, developed by RoboMaster and motivated by [RoboMaster AI Challenge](#competition) + +The framework of RoboRTS consists two parts: autonomous mobile robot layer and intelligent decision-making layer. + + + +The autonomous mobile robot layer alone can let a robot, offically supported for RoboMaster AI Robot platform, to demonstrate a certain level of intelligence. On its on-board computer runs perception, motion planning, decision modules. It is fully supported in ROS with community driven as well as platform-customized codes and examples. On its MCU, an RT low-level robot controller is implemented to govern the robot driving system. + +**TODO:** Intelligent decision-making layer includes a multi-agent decision-making framework and a game simulator, it will be released soon in the future. + +## Tutorial + +For more information about RoboMaster AI Robot platform and RoboRTS framework, please refer to [RoboRTS Tutorial](https://robomaster.github.io/RoboRTS-Tutorial/#/) + +## Competition + +RoboMaster holds AI Challenge since 2017. In the challenge, multiple robots should fight with each other on a game field automatically under different rules. + +For more information, please refer to + +- [DJI RoboMaster 2019 ICRA AI Challenge](https://icra2019.org/competitions/dji-robomaster-ai-challenge) + +- [DJI RoboMaster 2018 ICRA AI Challenge](https://icra2018.org/dji-robomaster-ai-challenge/) + +## Copyright and License + +RoboRTS is provided under the [GPL-v3](COPYING). diff --git a/images/robot.jpg b/images/robot.jpg new file mode 100644 index 00000000..702de095 Binary files /dev/null and b/images/robot.jpg differ diff --git a/images/system.png b/images/system.png new file mode 100755 index 00000000..a9e3ea6d Binary files /dev/null and b/images/system.png differ diff --git a/license.txt b/license.txt new file mode 100644 index 00000000..a40d675d --- /dev/null +++ b/license.txt @@ -0,0 +1,318 @@ +The RoboRTS is made available to you under the terms of GPL 3.0 or +later, a copy of the GPL 3.0 license is provided in COPYING. + +========================================================================= + +The RoboRTS uses libraries of costmap_2d (https://github.com/ros- +planning/navigation/tree/kinetic-devel/costmap_2d), which is licensed +under BSD license. A copy of the BSD license is provided below and is +also available at https://opensource.org/licenses/BSD-3-Clause. + + Copyright (c) 2008, 2013, Willow Garage, Inc. + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of Willow Garage, Inc. nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + +========================================================================== +The RoboRTS uses libraries of teb_local_planner (https://github.com/rst +-tu-dortmund/teb_local_planner), which is licensed under BSD 3-clause +license.A copy of the BSD 3-clause license is provided below and is also +available at https://opensource.org/licenses/BSD-3-Clause. + +Copyright (c) 2016, +TU Dortmund - Institute of Control Theory and Systems Engineering. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of teb_local_planner nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +========================================================================== + +The RoboRTS uses libraries of amcl (https://github.com/ros-planning/ +navigation/tree/kinetic-devel/amcl), which is licensed under LGPL +license. A copy of the LGPL license is provided below and is also +available at https://www.gnu.org/licenses/lgpl-3.0.txt. + + Copyright (c) 2008, Willow Garage, Inc. + All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Player - One Hell of a Robot Server +Copyright (C) 2000 Brian Gerkey & Kasper Stoy + gerkey@usc.edu kaspers@robotics.usc.edu + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + GNU LESSER 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. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser 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 +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. + +========================================================================== + +The RoboRTS uses libraries of KCFcpp (https://github.com/joaofaro/KCFcpp), +which is licensed under BSD license. A copy of the BSD license is provided +below and is also available at https://opensource.org/licenses/BSD-3-Clause. + +Copyright (c) 2015, Joao Faro +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of KCFcpp nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/roborts/package.xml b/roborts/package.xml new file mode 100644 index 00000000..05464045 --- /dev/null +++ b/roborts/package.xml @@ -0,0 +1,27 @@ + + roborts + 1.0.0 + + The roborts meta package provides all the functionality for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + roborts_base + roborts_bringup + roborts_camera + roborts_common + roborts_costmap + roborts_decision + roborts_localization + roborts_msgs + roborts_planning + roborts_tracking + + + + + \ No newline at end of file diff --git a/roborts_base/CMakeLists.txt b/roborts_base/CMakeLists.txt new file mode 100644 index 00000000..9ab85fc1 --- /dev/null +++ b/roborts_base/CMakeLists.txt @@ -0,0 +1,32 @@ +project(roborts_base) +cmake_minimum_required(VERSION 3.1) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) +find_package(Threads REQUIRED) +find_package(Glog REQUIRED) +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + roborts_msgs + ) + +catkin_package() + +add_executable(roborts_base_node + roborts_base_node.cpp + chassis/chassis.cpp + gimbal/gimbal.cpp + roborts_sdk/dispatch/execution.cpp + roborts_sdk/dispatch/handle.cpp + roborts_sdk/protocol/protocol.cpp + roborts_sdk/hardware/serial_device.cpp + ) +target_link_libraries(roborts_base_node PUBLIC + Threads::Threads + ${GLOG_LIBRARY} + ${catkin_LIBRARIES}) +target_include_directories(roborts_base_node PUBLIC + ${catkin_INCLUDE_DIRS}) +add_dependencies(roborts_base_node roborts_msgs_generate_messages) + diff --git a/roborts_base/chassis/chassis.cpp b/roborts_base/chassis/chassis.cpp new file mode 100644 index 00000000..152d21b4 --- /dev/null +++ b/roborts_base/chassis/chassis.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "chassis.h" +#include "../roborts_sdk/sdk.h" + +namespace roborts_base{ +Chassis::Chassis(std::shared_ptr handle): + handle_(handle){ + SDK_Init(); + ROS_Init(); +} +void Chassis::SDK_Init(){ + handle_->CreateSubscriber(CHASSIS_CMD_SET, PUSH_CHASSIS_INFO, + CHASSIS_ADDRESS, MANIFOLD2_ADDRESS, + std::bind(&Chassis::ChassisInfoCallback, this, std::placeholders::_1)); + handle_->CreateSubscriber(CHASSIS_CMD_SET, PUSH_UWB_INFO, + CHASSIS_ADDRESS, MANIFOLD2_ADDRESS, + std::bind(&Chassis::UWBInfoCallback, this, std::placeholders::_1)); + + chassis_speed_pub_ = handle_->CreatePublisher(CHASSIS_CMD_SET, CTRL_CHASSIS_SPEED, + MANIFOLD2_ADDRESS, CHASSIS_ADDRESS); + chassis_mode_pub_ = handle_->CreatePublisher(CHASSIS_CMD_SET, SET_CHASSIS_MODE, + MANIFOLD2_ADDRESS, CHASSIS_ADDRESS); + chassis_spd_acc_pub_ = handle_->CreatePublisher(CHASSIS_CMD_SET, CTRL_CHASSIS_SPEED_ACC, + MANIFOLD2_ADDRESS, CHASSIS_ADDRESS); + +} +void Chassis::ROS_Init(){ + //ros publisher + ros_odom_pub_ = ros_nh_.advertise("odom", 30); + ros_uwb_pub_ = ros_nh_.advertise("uwb", 30); + //ros subscriber + ros_sub_cmd_chassis_vel_ = ros_nh_.subscribe("cmd_vel", 1, &Chassis::ChassisSpeedCtrlCallback, this); + ros_sub_cmd_chassis_vel_acc_ = ros_nh_.subscribe("cmd_vel_acc", 1, &Chassis::ChassisSpeedAccCtrlCallback, this); + //ros service + ros_chassis_mode_srv_ = ros_nh_.advertiseService("set_chassis_mode", &Chassis::SetChassisModeService, this); + + //ros_message_init + odom_.header.frame_id = "odom"; + odom_.child_frame_id = "base_link"; + + odom_tf_.header.frame_id = "odom"; + odom_tf_.child_frame_id = "base_link"; + + uwb_data_.header.frame_id = "uwb"; +} +void Chassis::ChassisInfoCallback(const std::shared_ptr chassis_info){ + + ros::Time current_time = ros::Time::now(); + odom_.header.stamp = current_time; + odom_.pose.pose.position.x = chassis_info->position_x_mm/1000.; + odom_.pose.pose.position.y = chassis_info->position_y_mm/1000.; + odom_.pose.pose.position.z = 0.0; + geometry_msgs::Quaternion q = tf::createQuaternionMsgFromYaw(chassis_info->gyro_angle / 180.0 * M_PI); + odom_.pose.pose.orientation = q; + odom_.twist.twist.linear.x = chassis_info->v_x_mm / 1000.0; + odom_.twist.twist.linear.y = chassis_info->v_y_mm / 1000.0; + odom_.twist.twist.angular.z = chassis_info->gyro_palstance / 180.0 * M_PI; + ros_odom_pub_.publish(odom_); + + odom_tf_.header.stamp = current_time; + odom_tf_.transform.translation.x = chassis_info->position_x_mm/1000.; + odom_tf_.transform.translation.y = chassis_info->position_y_mm/1000.; + + odom_tf_.transform.translation.z = 0.0; + odom_tf_.transform.rotation = q; + tf_broadcaster_.sendTransform(odom_tf_); + +} +void Chassis::UWBInfoCallback(const std::shared_ptr uwb_info){ + + uwb_data_.header.stamp = ros::Time::now(); + uwb_data_.pose.position.x = ((double)uwb_info->x)/100.0; + uwb_data_.pose.position.y = ((double)uwb_info->y)/100.0; + uwb_data_.pose.position.z = 0; + uwb_data_.pose.orientation = tf::createQuaternionMsgFromYaw(uwb_info->yaw); + ros_uwb_pub_.publish(uwb_data_); + +} +bool Chassis::SetChassisModeService(roborts_msgs::ChassisMode::Request &req, + roborts_msgs::ChassisMode::Response &res){ + + roborts_sdk::chassis_mode_e chassis_mode = + static_cast(req.chassis_mode); + chassis_mode_pub_->Publish(chassis_mode); + + res.received = true; + return true; +} +void Chassis::ChassisSpeedCtrlCallback(const geometry_msgs::Twist::ConstPtr &vel){ + roborts_sdk::p_chassis_speed_t chassis_speed; + chassis_speed.vx = vel->linear.x*1000; + chassis_speed.vy = vel->linear.y*1000; + chassis_speed.vw = vel->angular.z * 180.0 / M_PI; + chassis_speed.rotate_x_offset = 0; + chassis_speed.rotate_y_offset = 0; + chassis_speed_pub_->Publish(chassis_speed); +} + +void Chassis::ChassisSpeedAccCtrlCallback(const roborts_msgs::TwistAccel::ConstPtr &vel_acc){ + roborts_sdk::p_chassis_spd_acc_t chassis_spd_acc; + chassis_spd_acc.vx = vel_acc->twist.linear.x*1000; + chassis_spd_acc.vy = vel_acc->twist.linear.y*1000; + chassis_spd_acc.vw = vel_acc->twist.angular.z * 180.0 / M_PI; + chassis_spd_acc.ax = vel_acc->accel.linear.x*1000; + chassis_spd_acc.ay = vel_acc->accel.linear.y*1000; + chassis_spd_acc.wz = vel_acc->accel.angular.z * 180.0 / M_PI; + chassis_spd_acc_pub_->Publish(chassis_spd_acc); +} +} diff --git a/roborts_base/chassis/chassis.h b/roborts_base/chassis/chassis.h new file mode 100644 index 00000000..4b0f5d36 --- /dev/null +++ b/roborts_base/chassis/chassis.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_BASE_CHASSIS_H +#define ROBORTS_BASE_CHASSIS_H +#include "../roborts_sdk/sdk.h" +#include "../ros_dep.h" + +namespace roborts_base { +/** + * @brief ROS API for chassis module + */ +class Chassis { + public: + /** + * @brief Constructor of chassis including initialization of sdk and ROS + * @param handle handler of sdk + */ + Chassis(std::shared_ptr handle); + + /** + * @brief Destructor of chassis + */ + ~Chassis() = default; + + private: + /** + * @brief Initialization of sdk + */ + void SDK_Init(); + + /** + * @brief Initialization of ROS + */ + void ROS_Init(); + + /** + * @brief Chassis information callback in sdk + * @param chassis_info Chassis information + */ + void ChassisInfoCallback(const std::shared_ptr chassis_info); + + /** + * @brief UWB information callback in sdk + * @param uwb_info UWB information + */ + void UWBInfoCallback(const std::shared_ptr uwb_info); + + /** + * @brief Chassis mode set service callback in ROS + * @param req Chassis mode set as request + * @param res Mode set result as response + * @return True if success + */ + bool SetChassisModeService(roborts_msgs::ChassisMode::Request &req, + roborts_msgs::ChassisMode::Response &res); + /** + * @brief Chassis speed control callback in ROS + * @param vel Chassis speed control data + */ + void ChassisSpeedCtrlCallback(const geometry_msgs::Twist::ConstPtr &vel); + + /** + * @brief Chassis speed and acceleration control callback in ROS + * @param vel_acc Chassis speed and acceleration control data + */ + void ChassisSpeedAccCtrlCallback(const roborts_msgs::TwistAccel::ConstPtr &vel_acc); + + //! sdk handler + std::shared_ptr handle_; + //! sdk publisher for chassis speed control + std::shared_ptr> chassis_speed_pub_; + //! sdk publisher for chassis mode control + std::shared_ptr> chassis_mode_pub_; + //! sdk publisher for chassis speed and acceleration control + std::shared_ptr> chassis_spd_acc_pub_; + + //! ros node handler + ros::NodeHandle ros_nh_; + //! ros subscriber for speed control + ros::Subscriber ros_sub_cmd_chassis_vel_; + //! ros subscriber for chassis speed and acceleration control + ros::Subscriber ros_sub_cmd_chassis_vel_acc_; + //! ros publisher for odometry information + ros::Publisher ros_odom_pub_; + //! ros publisher for uwb information + ros::Publisher ros_uwb_pub_; + //! ros service server for chassis mode set + ros::ServiceServer ros_chassis_mode_srv_; + + //! ros chassis odometry tf + geometry_msgs::TransformStamped odom_tf_; + //! ros chassis odometry tf broadcaster + tf::TransformBroadcaster tf_broadcaster_; + //! ros odometry message + nav_msgs::Odometry odom_; + //! ros uwb message + geometry_msgs::PoseStamped uwb_data_; +}; +} +#endif //ROBORTS_BASE_CHASSIS_H diff --git a/roborts_base/cmake_module/FindGlog.cmake b/roborts_base/cmake_module/FindGlog.cmake new file mode 100755 index 00000000..99abbe47 --- /dev/null +++ b/roborts_base/cmake_module/FindGlog.cmake @@ -0,0 +1,48 @@ +# - Try to find Glog +# +# The following variables are optionally searched for defaults +# GLOG_ROOT_DIR: Base directory where all GLOG components are found +# +# The following are set after configuration is done: +# GLOG_FOUND +# GLOG_INCLUDE_DIRS +# GLOG_LIBRARIES +# GLOG_LIBRARYRARY_DIRS + +include(FindPackageHandleStandardArgs) + +set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") + +if(WIN32) + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}/src/windows) +else() + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}) +endif() + +if(MSVC) + find_library(GLOG_LIBRARY_RELEASE libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Release) + + find_library(GLOG_LIBRARY_DEBUG libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Debug) + + set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) +else() + find_library(GLOG_LIBRARY glog + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES lib lib64) +endif() + +find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) + +if(GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) + message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") + mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG + GLOG_LIBRARY GLOG_INCLUDE_DIR) +endif() diff --git a/roborts_base/config/roborts_base_parameter.yaml b/roborts_base/config/roborts_base_parameter.yaml new file mode 100644 index 00000000..12092d04 --- /dev/null +++ b/roborts_base/config/roborts_base_parameter.yaml @@ -0,0 +1 @@ +serial_port : "/dev/serial_sdk" \ No newline at end of file diff --git a/roborts_base/gimbal/gimbal.cpp b/roborts_base/gimbal/gimbal.cpp new file mode 100644 index 00000000..6850d3de --- /dev/null +++ b/roborts_base/gimbal/gimbal.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "gimbal.h" +#include "../roborts_sdk/sdk.h" + +namespace roborts_base{ +Gimbal::Gimbal(std::shared_ptr handle): + handle_(handle){ + SDK_Init(); + ROS_Init(); +} + +void Gimbal::SDK_Init(){ + handle_->CreateSubscriber(GIMBAL_CMD_SET, PUSH_GIMBAL_INFO, + GIMBAL_ADDRESS, BROADCAST_ADDRESS, + std::bind(&Gimbal::GimbalInfoCallback, this, std::placeholders::_1)); + + gimbal_angle_pub_ = handle_->CreatePublisher(GIMBAL_CMD_SET, CTRL_GIMBAL_ANGLE, + MANIFOLD2_ADDRESS, GIMBAL_ADDRESS); + gimbal_rate_pub_ = handle_->CreatePublisher(GIMBAL_CMD_SET, CTRL_GIMBAL_RATE, + MANIFOLD2_ADDRESS, GIMBAL_ADDRESS); + gimbal_mode_pub_= handle_->CreatePublisher(GIMBAL_CMD_SET, SET_GIMBAL_MODE, + MANIFOLD2_ADDRESS, GIMBAL_ADDRESS); + fric_wheel_pub_= handle_->CreatePublisher(GIMBAL_CMD_SET, CTRL_FRIC_WHEEL_SPEED, + MANIFOLD2_ADDRESS, GIMBAL_ADDRESS); + gimbal_shoot_pub_= handle_->CreatePublisher(GIMBAL_CMD_SET, CTRL_GIMBAL_SHOOT, + MANIFOLD2_ADDRESS, GIMBAL_ADDRESS); + +} + +void Gimbal::ROS_Init(){ + + //ros subscriber + ros_sub_cmd_gimbal_angle_ = ros_nh_.subscribe("cmd_gimbal_angle", 1, &Gimbal::GimbalAngleCtrlCallback, this); + + ros_sub_cmd_gimbal_rate_ = ros_nh_.subscribe("cmd_gimbal_rate", 1, &Gimbal::GimbalRateCtrlCallback, this); + //ros service + ros_gimbal_mode_srv_ = ros_nh_.advertiseService("set_gimbal_mode", &Gimbal::SetGimbalModeService, this); + ros_ctrl_fric_wheel_srv_ = ros_nh_.advertiseService("cmd_fric_wheel", &Gimbal::CtrlFricWheelService, this); + ros_ctrl_shoot_srv_ = ros_nh_.advertiseService("cmd_shoot", &Gimbal::CtrlShootService, this); + //ros_message_init + gimbal_tf_.header.frame_id = "base_link"; + gimbal_tf_.child_frame_id = "gimbal"; + +} + +void Gimbal::GimbalInfoCallback(const std::shared_ptr gimbal_info){ + + ros::Time current_time = ros::Time::now(); + geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0.0, + gimbal_info->pit_relative_angle / 180.0 * M_PI, + gimbal_info->yaw_relative_angle / 180.0 * M_PI); + gimbal_tf_.header.stamp = current_time; + gimbal_tf_.transform.rotation = q; + gimbal_tf_.transform.translation.x = 0; + gimbal_tf_.transform.translation.y = 0; + gimbal_tf_.transform.translation.z = 0.15; + tf_broadcaster_.sendTransform(gimbal_tf_); + +} + +void Gimbal::GimbalAngleCtrlCallback(const roborts_msgs::GimbalAngle::ConstPtr &msg){ + + roborts_sdk::p_gimbal_angle_t gimbal_angle; + gimbal_angle.ctrl.bit.pitch_mode = msg->pitch_mode; + gimbal_angle.ctrl.bit.yaw_mode = msg->yaw_mode; + gimbal_angle.pit_relative = msg->pitch_angle*180/M_PI; + gimbal_angle.pit_absolute = msg->pitch_angle*180/M_PI; + + gimbal_angle.yaw_absolute = msg->yaw_angle*180/M_PI; + gimbal_angle.yaw_relative = msg->yaw_angle*180/M_PI; + gimbal_angle_pub_->Publish(gimbal_angle); + +} + + + +void Gimbal::GimbalRateCtrlCallback(const roborts_msgs::GimbalRate::ConstPtr &msg){ + + roborts_sdk::p_gimbal_speed_t gimbal_rate; + gimbal_rate.pit_rate = msg->pitch_rate*180/M_PI; + gimbal_rate.pit_time = 1; + gimbal_rate.yaw_rate = msg->yaw_rate*180/M_PI; + gimbal_rate.yaw_time = 1; + gimbal_rate_pub_->Publish(gimbal_rate); + +} + +bool Gimbal::SetGimbalModeService(roborts_msgs::GimbalMode::Request &req, + roborts_msgs::GimbalMode::Response &res){ + roborts_sdk::gimbal_mode_e gimbal_mode = static_cast(req.gimbal_mode); + gimbal_mode_pub_->Publish(gimbal_mode); + res.received = true; + return true; +} +bool Gimbal::CtrlFricWheelService(roborts_msgs::FricWhl::Request &req, + roborts_msgs::FricWhl::Response &res){ + roborts_sdk::p_fric_wheel_speed_t fric_speed; + if(req.open){ + fric_speed = 1200; + } else{ + fric_speed = 1000; + } + fric_wheel_pub_->Publish(fric_speed); + res.received = true; + return true; +} +bool Gimbal::CtrlShootService(roborts_msgs::ShootCmd::Request &req, + roborts_msgs::ShootCmd::Response &res){ + roborts_sdk::p_gimbal_shoot_t gimbal_shoot; + uint16_t default_freq = 1500; + switch(static_cast(req.mode)){ + case roborts_sdk::SHOOT_STOP: + gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_STOP; + gimbal_shoot.shoot_add_num = 0; + gimbal_shoot.shoot_freq = 0; + break; + case roborts_sdk::SHOOT_ONCE: + if(req.number!=0){ + gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_ONCE; + gimbal_shoot.shoot_add_num = req.number; + gimbal_shoot.shoot_freq = default_freq; + } + else{ + gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_ONCE; + gimbal_shoot.shoot_add_num = 1; + gimbal_shoot.shoot_freq = default_freq; + } + break; + case roborts_sdk::SHOOT_CONTINUOUS: + gimbal_shoot.shoot_cmd = roborts_sdk::SHOOT_CONTINUOUS; + gimbal_shoot.shoot_add_num = req.number; + gimbal_shoot.shoot_freq = default_freq; + break; + default: + return false; + } + gimbal_shoot_pub_->Publish(gimbal_shoot); + + res.received = true; + return true; +} +} \ No newline at end of file diff --git a/roborts_base/gimbal/gimbal.h b/roborts_base/gimbal/gimbal.h new file mode 100644 index 00000000..10d50854 --- /dev/null +++ b/roborts_base/gimbal/gimbal.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_BASE_GIMBAL_H +#define ROBORTS_BASE_GIMBAL_H +#include "../roborts_sdk/sdk.h" +#include "../ros_dep.h" + +namespace roborts_base { +/** + * @brief ROS API for gimbal module + */ +class Gimbal { + public: + /** + * @brief Constructor of gimbal including initialization of sdk and ROS + * @param handle handler of sdk + */ + Gimbal(std::shared_ptr handle); + /** + * @brief Destructor of gimbal + */ + ~Gimbal() = default; + private: + /** + * @brief Initialization of sdk + */ + void SDK_Init(); + /** + * @brief Initialization of ROS + */ + void ROS_Init(); + + /** + * @brief Gimbal information callback in sdk + * @param gimbal_info Gimbal information + */ + void GimbalInfoCallback(const std::shared_ptr gimbal_info); + /** + * @brief Gimbal angle control callback in ROS + * @param msg Gimbal angle control data + */ + void GimbalAngleCtrlCallback(const roborts_msgs::GimbalAngle::ConstPtr &msg); + + /** + * @brief Gimbal rate control callback in ROS + * @param msg Gimbal rate control data + */ + void GimbalRateCtrlCallback(const roborts_msgs::GimbalRate::ConstPtr &msg); + /** + * @brief Gimbal mode set service callback in ROS + * @param req Gimbal mode set as request + * @param res Mode set result as response + * @return True if success + */ + bool SetGimbalModeService(roborts_msgs::GimbalMode::Request &req, + roborts_msgs::GimbalMode::Response &res); + /** + * @brief Control friction wheel service callback in ROS + * @param req Friction wheel control data as request + * @param res Control result as response + * @return True if success + */ + bool CtrlFricWheelService(roborts_msgs::FricWhl::Request &req, + roborts_msgs::FricWhl::Response &res); + /** + * @brief Control shoot service callback in ROS + * @param req Shoot control data as request + * @param res Control result as response + * @return True if success + */ + bool CtrlShootService(roborts_msgs::ShootCmd::Request &req, + roborts_msgs::ShootCmd::Response &res); + + //! sdk handler + std::shared_ptr handle_; + //! sdk publisher for gimbal angle control + std::shared_ptr> gimbal_angle_pub_; + //! sdk publisher for gimbal rate control + std::shared_ptr> gimbal_rate_pub_; + //! sdk publisher for gimbal mode set + std::shared_ptr> gimbal_mode_pub_; + //! sdk publisher for frcition wheel control + std::shared_ptr> fric_wheel_pub_; + //! sdk publisher for gimbal shoot control + std::shared_ptr> gimbal_shoot_pub_; + + //! ros node handler + ros::NodeHandle ros_nh_; + //! ros subscriber for gimbal angle control + ros::Subscriber ros_sub_cmd_gimbal_angle_; + //! ros subscriber for gimbal rate control + ros::Subscriber ros_sub_cmd_gimbal_rate_; + //! ros service server for gimbal mode set + ros::ServiceServer ros_gimbal_mode_srv_; + //! ros service server for friction wheel control + ros::ServiceServer ros_ctrl_fric_wheel_srv_; + //! ros service server for gimbal shoot control + ros::ServiceServer ros_ctrl_shoot_srv_; + //! ros gimbal tf + geometry_msgs::TransformStamped gimbal_tf_; + //! ros gimbal tf broadcaster + tf::TransformBroadcaster tf_broadcaster_; + +}; +} +#endif //ROBORTS_BASE_GIMBAL_H diff --git a/roborts_base/package.xml b/roborts_base/package.xml new file mode 100755 index 00000000..0ae9587b --- /dev/null +++ b/roborts_base/package.xml @@ -0,0 +1,24 @@ + + roborts_base + 1.0.0 + + The roborts_base package provides protocol sdk for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + roscpp + rospy + tf + roborts_msgs + + roscpp + rospy + tf + roborts_msgs + + diff --git a/roborts_base/roborts_base_config.h b/roborts_base/roborts_base_config.h new file mode 100644 index 00000000..fc2368a7 --- /dev/null +++ b/roborts_base/roborts_base_config.h @@ -0,0 +1,32 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_BASE_CONFIG_H +#define ROBORTS_BASE_CONFIG_H +#include + +namespace roborts_base{ + +struct Config { + void GetParam(ros::NodeHandle *nh) { + nh->param("serial_port", serial_port, "/dev/serial_sdk"); + } + std::string serial_port; +}; + +} +#endif //ROBORTS_BASE_CONFIG_H \ No newline at end of file diff --git a/roborts_base/roborts_base_node.cpp b/roborts_base/roborts_base_node.cpp new file mode 100644 index 00000000..d6c94bf0 --- /dev/null +++ b/roborts_base/roborts_base_node.cpp @@ -0,0 +1,37 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "gimbal/gimbal.h" +#include "chassis/chassis.h" +#include "roborts_base_config.h" + +int main(int argc, char **argv){ + ros::init(argc, argv, "roborts_base_node"); + ros::NodeHandle nh; + roborts_base::Config config; + config.GetParam(&nh); + auto handle = std::make_shared(config.serial_port); + if(!handle->Init()) return 1; + + roborts_base::Chassis chassis(handle); + roborts_base::Gimbal gimbal(handle); + while(ros::ok()){ + handle->Spin(); + ros::spinOnce(); + usleep(1000); + } +} \ No newline at end of file diff --git a/roborts_base/roborts_sdk/dispatch/dispatch.h b/roborts_base/roborts_sdk/dispatch/dispatch.h new file mode 100644 index 00000000..9adbc347 --- /dev/null +++ b/roborts_base/roborts_sdk/dispatch/dispatch.h @@ -0,0 +1,348 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_DISPATCH_H +#define ROBORTS_SDK_DISPATCH_H +#include // std::cout +#include // std::ref +#include // std::thread +#include // std::promise, std::future +#include +#include + +#include "handle.h" + +namespace roborts_sdk { +class Handle; + +template +class SubscriptionCallback { + public: + using SharedMessage = typename std::shared_ptr; + using CallbackType = typename std::function; + + SubscriptionCallback(CallbackType function) : + callback_function_(function) { + + } + ~SubscriptionCallback() = default; + void Dispatch(std::shared_ptr message_header, + SharedMessage message) { + callback_function_(message); + }; + private: + CallbackType callback_function_; +}; + +template +class ServiceCallback { + public: + using SharedRequest = typename std::shared_ptr; + using SharedResponse = typename std::shared_ptr; + using CallbackType = typename std::function; + ServiceCallback(CallbackType function) : + callback_function_(function) { + + } + ~ServiceCallback() = default; + void Dispatch(std::shared_ptr message_header, + SharedRequest request, + SharedResponse response) { + callback_function_(request, response); + }; + private: + CallbackType callback_function_; +}; + +class SubscriptionBase { + public: + SubscriptionBase(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) : + handle_(handle) { + cmd_info_ = std::make_shared(); + cmd_info_->cmd_id = cmd_id; + cmd_info_->cmd_set = cmd_set; + cmd_info_->receiver = receiver; + cmd_info_->sender = sender; + cmd_info_->need_ack = false; + } + ~SubscriptionBase() = default; + std::shared_ptr GetHandle() { + return handle_; + } + std::shared_ptr GetCommandInfo() { + return cmd_info_; + } + std::shared_ptr CreateMessageHeader() { + return std::shared_ptr(new MessageHeader); + } + + virtual std::shared_ptr CreateMessage() = 0; + virtual void HandleMessage(std::shared_ptr message_header, std::shared_ptr message) = 0; + protected: + std::shared_ptr handle_; + std::shared_ptr cmd_info_; + +}; + +template +class Subscription : public SubscriptionBase { + public: + using SharedMessage = typename std::shared_ptr; + using CallbackType = typename std::function; + + Subscription(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver, + CallbackType &&function) : + SubscriptionBase(handle, cmd_set, cmd_id, sender, receiver), + callback_(std::forward(function)) { + cmd_info_->length = sizeof(Cmd); + } + ~Subscription() = default; + std::shared_ptr CreateMessage() { + return std::shared_ptr(new Cmd); + } + void HandleMessage(std::shared_ptr message_header, std::shared_ptr message) { + auto typed_message = std::static_pointer_cast(message); + callback_.Dispatch(message_header, typed_message); + } + private: + SubscriptionCallback callback_; +}; + +class PublisherBase { + public: + PublisherBase(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) : + handle_(handle) { + cmd_info_ = std::make_shared(); + cmd_info_->cmd_id = cmd_id; + cmd_info_->cmd_set = cmd_set; + cmd_info_->receiver = receiver; + cmd_info_->sender = sender; + cmd_info_->need_ack = false; + } + ~PublisherBase() = default; + std::shared_ptr GetHandle() { + return handle_; + } + std::shared_ptr GetCommandInfo() { + return cmd_info_; + } + + protected: + std::shared_ptr handle_; + std::shared_ptr cmd_info_; +}; + +template +class Publisher : public PublisherBase { + public: + Publisher(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) : + PublisherBase(handle, cmd_set, cmd_id, sender, receiver) { + cmd_info_->length = sizeof(Cmd); + } + ~Publisher() = default; + + void Publish(Cmd &message) { + bool ret = GetHandle()->GetProtocol()->SendMessage(GetCommandInfo().get(), &message); + if (!ret) { + DLOG_ERROR << "send message failed!"; + } + } +}; + +class ClientBase { + public: + ClientBase(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) : + handle_(handle) { + cmd_info_ = std::make_shared(); + cmd_info_->cmd_id = cmd_id; + cmd_info_->cmd_set = cmd_set; + cmd_info_->receiver = receiver; + cmd_info_->sender = sender; + cmd_info_->need_ack = true; + } + ~ClientBase() = default; + std::shared_ptr GetHandle() { + return handle_; + } + std::shared_ptr GetCommandInfo() { + return cmd_info_; + } + std::shared_ptr CreateRequestHeader() { + return std::shared_ptr(new MessageHeader); + } + + virtual std::shared_ptr CreateResponse() = 0; + virtual void HandleResponse(std::shared_ptr request_header, + std::shared_ptr response) = 0; + + protected: + std::shared_ptr handle_; + std::shared_ptr cmd_info_; + +}; + +template +class Client : public ClientBase { + public: + using SharedRequest = typename std::shared_ptr; + using SharedResponse = typename std::shared_ptr; + + using Promise = std::promise; + using SharedPromise = std::shared_ptr; + using SharedFuture = std::shared_future; + using CallbackType = std::function; + + Client(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) : + ClientBase(handle, cmd_set, cmd_id, sender, receiver) { + cmd_info_->length = sizeof(Ack); + } + ~Client() = default; + + std::shared_ptr CreateResponse() { + return std::shared_ptr(new Ack()); + } + + void HandleResponse(std::shared_ptr request_header, std::shared_ptr response) { + + std::unique_lock lock(pending_requests_mutex_); + auto typed_response = std::static_pointer_cast(response); + + auto call_promise = std::get<0>(pending_requests_); + auto callback = std::get<1>(pending_requests_); + auto future = std::get<2>(pending_requests_); + // Unlock here to allow the service to be called recursively from one of its callbacks. + lock.unlock(); + + call_promise->set_value(typed_response); + callback(future); + } + + SharedFuture + AsyncSendRequest(SharedRequest request) { + return AsyncSendRequest(request, [](SharedFuture) {}); + } + + template + SharedFuture AsyncSendRequest(SharedRequest request, CallbackType &&cb) { + std::lock_guard lock(pending_requests_mutex_); + bool ret = GetHandle()->GetProtocol()->SendRequest(GetCommandInfo().get(), request.get()); + if (!ret) { + DLOG_ERROR << "async_send_request failed!"; + } + + SharedPromise call_promise = std::make_shared(); + SharedFuture f(call_promise->get_future()); + pending_requests_ = std::make_tuple(call_promise, std::forward(cb), f); + return f; + } + + private: + std::tuple pending_requests_; + std::mutex pending_requests_mutex_; +}; + +class ServiceBase { + public: + ServiceBase(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) : + handle_(handle) { + cmd_info_ = std::make_shared(); + cmd_info_->cmd_id = cmd_id; + cmd_info_->cmd_set = cmd_set; + cmd_info_->receiver = receiver; + cmd_info_->sender = sender; + cmd_info_->need_ack = true; + } + ~ServiceBase() = default; + + std::shared_ptr GetHandle() { + return handle_; + } + std::shared_ptr GetCommandInfo() { + return cmd_info_; + } + std::shared_ptr CreateRequestHeader() { + return std::shared_ptr(new MessageHeader); + } + + virtual std::shared_ptr CreateRequest() = 0; + virtual void HandleRequest( + std::shared_ptr request_header, + std::shared_ptr request) = 0; + protected: + std::shared_ptr handle_; + std::shared_ptr cmd_info_; +}; + +template +class Service : public ServiceBase { + + public: + using SharedRequest = typename std::shared_ptr; + using SharedResponse = typename std::shared_ptr; + using CallbackType = typename std::function; + + Service(std::shared_ptr handle, + uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver, + CallbackType &&function) : + ServiceBase(handle, cmd_set, cmd_id, sender, receiver), + callback_(std::forward(function)) { + cmd_info_->length = sizeof(Cmd); + } + ~Service() = default; + + std::shared_ptr CreateRequest() { + return std::shared_ptr(new Cmd()); + } + void HandleRequest( + std::shared_ptr request_header, + std::shared_ptr request) { + auto typed_request = std::static_pointer_cast(request); + auto typed_response = std::shared_ptr(new Ack); + callback_.Dispatch(request_header, typed_request, typed_response); + SendResponse(request_header, typed_response); + } + + void SendResponse( + std::shared_ptr request_header, + std::shared_ptr response) { + bool ret = GetHandle()->GetProtocol()->SendResponse(GetCommandInfo().get(), request_header.get(), response.get()); + + if (!ret) { + DLOG_ERROR << "send response failed!"; + } + } + + private: + ServiceCallback callback_; +}; +} +#endif //ROBORTS_SDK_DISPATCH_H diff --git a/roborts_base/roborts_sdk/dispatch/execution.cpp b/roborts_base/roborts_sdk/dispatch/execution.cpp new file mode 100644 index 00000000..3378c14c --- /dev/null +++ b/roborts_base/roborts_sdk/dispatch/execution.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "execution.h" + +namespace roborts_sdk { +Executor::Executor(std::shared_ptr handle) : handle_(handle) {} +std::shared_ptr Executor::GetHandle() { + return handle_; +} + +void Executor::ExecuteSubscription(std::shared_ptr subscription) { + auto message_header = subscription->CreateMessageHeader(); + std::shared_ptr message = subscription->CreateMessage(); + + bool ret = GetHandle()->GetProtocol()->Take( + subscription->GetCommandInfo().get(), + message_header.get(), + message.get()); + if (ret) { + subscription->HandleMessage(message_header, message); + } else { +// DLOG_ERROR<<"take message failed!"; + } + //TODO: add return message; + //subscription->return_message(message); +} +void Executor::ExecuteService(std::shared_ptr service) { + auto request_header = service->CreateRequestHeader(); + std::shared_ptr request = service->CreateRequest(); + + bool ret = GetHandle()->GetProtocol()->Take( + service->GetCommandInfo().get(), + request_header.get(), + request.get()); + if (ret) { + service->HandleRequest(request_header, request); + } else { + DLOG_ERROR << "take request failed!"; + } +} +void Executor::ExecuteClient(std::shared_ptr client) { + auto request_header = client->CreateRequestHeader(); + std::shared_ptr response = client->CreateResponse(); + + bool ret = GetHandle()->GetProtocol()->Take( + client->GetCommandInfo().get(), + request_header.get(), + response.get()); + + if (ret) { + client->HandleResponse(request_header, response); + } else { +// DLOG_ERROR<<"take response failed!"; + } +} +} \ No newline at end of file diff --git a/roborts_base/roborts_sdk/dispatch/execution.h b/roborts_base/roborts_sdk/dispatch/execution.h new file mode 100644 index 00000000..be1a4147 --- /dev/null +++ b/roborts_base/roborts_sdk/dispatch/execution.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_EXECUTION_H +#define ROBORTS_SDK_EXECUTION_H +#include "dispatch.h" + +namespace roborts_sdk { +class Handle; +class SubscriptionBase; +class PublisherBase; +class ClientBase; +class ServiceBase; + +/** + * @brief Executor class to provide execute interface for subscriber, service server and client in the dispatch layer + */ +class Executor { + public: + /** + * @brief Constructor of Executor + * @param handle Pointer of handle which consists of handler of executor, protocol layer and hardware layer + */ + Executor(std::shared_ptr handle); + ~Executor() = default; + /** + * @brief Get the handle + * @return Pointer of handle + */ + std::shared_ptr GetHandle(); + /** + * @brief Given the subscription, invoke its callback function + * @param subscription Subscription base pointer of certain command + */ + void ExecuteSubscription(std::shared_ptr subscription); + /** + * @brief Given the service, invoke its callback function and call the protocol layer to send response/ack + * @param service Service base pointer of certain command + */ + void ExecuteService(std::shared_ptr service); + /** + * @brief Given the client, call the protocol layer to send command and wait for the response/ack + * @param client Client base pointer of certain command + */ + void ExecuteClient(std::shared_ptr client); + + private: + //! pointer of handle + std::shared_ptr handle_; +}; +} +#endif //ROBORTS_SDK_EXECUTION_H diff --git a/roborts_base/roborts_sdk/dispatch/handle.cpp b/roborts_base/roborts_sdk/dispatch/handle.cpp new file mode 100644 index 00000000..13dddf75 --- /dev/null +++ b/roborts_base/roborts_sdk/dispatch/handle.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "handle.h" + +namespace roborts_sdk { +Handle::Handle(std::string serial_port) { + device_ = std::make_shared(serial_port, 921600); + protocol_ = std::make_shared(device_); + +} +bool Handle::Init(){ + if (!device_->Init()) { + LOG_ERROR<<"Can not open device. "; + return false; + } + if (!protocol_->Init()) { + LOG_ERROR<<"Protocol initialization failed."; + return false; + } + return true; +} +std::shared_ptr Handle::GetProtocol() { + return protocol_; +} + +void Handle::Spin() { + executor_ = std::make_shared(shared_from_this()); + for (auto sub :subscription_factory_) { + executor_->ExecuteSubscription(sub); + } + for (auto client :client_factory_) { + executor_->ExecuteClient(client); + } + for (auto service :service_factory_) { + executor_->ExecuteService(service); + } +} +} \ No newline at end of file diff --git a/roborts_base/roborts_sdk/dispatch/handle.h b/roborts_base/roborts_sdk/dispatch/handle.h new file mode 100644 index 00000000..177543df --- /dev/null +++ b/roborts_base/roborts_sdk/dispatch/handle.h @@ -0,0 +1,179 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_HANDLE_H +#define ROBORTS_SDK_HANDLE_H +#include "../protocol/protocol.h" +#include "dispatch.h" +#include "execution.h" + +namespace roborts_sdk { +class SubscriptionBase; +class PublisherBase; +class ClientBase; +class ServiceBase; +class Executor; + +template +class Subscription; +template +class Publisher; +template +class Client; +template +class Service; +/** + * @brief Handle class in the dispatch layer + */ +class Handle : public std::enable_shared_from_this { + //TODO: make this singleton + public: + template + friend + class Subscription; + template + friend + class Publisher; + template + friend + class Client; + template + friend + class Service; + /** + * @brief Constructor of Handle, instantiate the object of the hardware layer and protocol layer + * @param serial_port + */ + explicit Handle(std::string serial_port); + /** + * @brief Initialize the hardware layer and protocol layer + * @return True if both initialize successfully; + */ + bool Init(); + /** + * @brief Get the pointer of protocol layer + * @return The pointer of protocol layer + */ + std::shared_ptr GetProtocol(); + /** + * @brief Create the subscriber for the protocol command without need of ack (Receive command) + * @tparam Cmd Command DataType + * @param cmd_set Command set for different module, i.e. gimbal, chassis + * @param cmd_id Command id for different commands in the module + * @param sender Sender address + * @param receiver Receiver address + * @param function Subscriber Callback function + * @return Pointer of subscription handle + */ + template + std::shared_ptr> CreateSubscriber(uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver, + typename Subscription::CallbackType &&function) { + auto subscriber = std::make_shared>(shared_from_this(), + cmd_set, cmd_id, + sender, receiver, + std::forward::CallbackType>( + function)); + subscription_factory_.push_back( + std::dynamic_pointer_cast(subscriber)); + return subscriber; + } + /** + * @brief Create the publisher for the protocol command without need of ack (Send command) + * @tparam Cmd Command DataType + * @param cmd_set Command set for different module, i.e. gimbal, chassis + * @param cmd_id Command id for different commands in the module + * @param sender Sender address + * @param receiver Receiver address + * @return Pointer of publisher handle + */ + template + std::shared_ptr> CreatePublisher(uint8_t cmd_set, uint8_t cmd_id, uint8_t sender, uint8_t receiver) { + auto publisher = std::make_shared>(shared_from_this(), + cmd_set, cmd_id, + sender, receiver); + publisher_factory_.push_back( + std::dynamic_pointer_cast(publisher)); + return publisher; + } + /** + * @brief Create the service for the protocol command with need of ack (Receive command and send ack) + * @tparam Cmd Command DataType + * @tparam Ack Ack DataType + * @param cmd_set Command set for different module, i.e. gimbal, chassis + * @param cmd_id Command id for different commands in the module + * @param sender Sender address + * @param receiver Receiver address + * @param function Server Callback function (Input command and output ack) + * @return Pointer of service handle + */ + template + std::shared_ptr> CreateServer(uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver, + typename Service::CallbackType &&function) { + auto service = std::make_shared>(shared_from_this(), + cmd_set, cmd_id, + sender, receiver, + std::forward::CallbackType>(function)); + service_factory_.push_back( + std::dynamic_pointer_cast(service)); + return service; + } + /** + * @brief Create the client for the protocol command with need of ack (Send command and wait for ack) + * @tparam Cmd Command DataType + * @tparam Ack Ack DataType + * @param cmd_set Command set for different module, i.e. gimbal, chassis + * @param cmd_id Command id for different commands in the module + * @param sender Sender address + * @param receiver Receiver address + * @return Pointer of client handle + */ + template + std::shared_ptr> CreateClient(uint8_t cmd_set, uint8_t cmd_id, + uint8_t sender, uint8_t receiver) { + auto client = std::make_shared>(shared_from_this(), + cmd_set, cmd_id, + sender, receiver); + client_factory_.push_back( + std::dynamic_pointer_cast(client)); + return client; + } + /** + * @brief Execute all the handlers to spin the loop + */ + void Spin(); + private: + //! vector of subsctription base pointers + std::vector> subscription_factory_; + //! vector of publisher base pointers + std::vector> publisher_factory_; + //! vector of service base pointers + std::vector> service_factory_; + //! vector of client base pointers + std::vector> client_factory_; + + //! executor pointer + std::shared_ptr executor_; + //! pointer of hardware layer + std::shared_ptr device_; + //! pointer of protocol layer + std::shared_ptr protocol_; +}; +} +#endif //ROBORTS_SDK_HANDLE_H diff --git a/roborts_base/roborts_sdk/hardware/hardware_interface.h b/roborts_base/roborts_sdk/hardware/hardware_interface.h new file mode 100644 index 00000000..dba6b070 --- /dev/null +++ b/roborts_base/roborts_sdk/hardware/hardware_interface.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_HARDWARE_INTERFACE_H +#define ROBORTS_SDK_HARDWARE_INTERFACE_H + +namespace roborts_sdk{ +/** + * @brief Abstract class for hardware as an interface + */ +class HardwareInterface{ + public: + HardwareInterface(){}; + virtual ~HardwareInterface() = default; + protected: + virtual bool Init() = 0; + virtual int Read(uint8_t *buf, int len) = 0; + virtual int Write(const uint8_t *buf, int len) = 0; +}; +} +#endif //ROBORTS_SDK_HARDWARE_INTERFACE_H diff --git a/roborts_base/roborts_sdk/hardware/serial_device.cpp b/roborts_base/roborts_sdk/hardware/serial_device.cpp new file mode 100644 index 00000000..6026b51c --- /dev/null +++ b/roborts_base/roborts_sdk/hardware/serial_device.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "serial_device.h" + +namespace roborts_sdk { +SerialDevice::SerialDevice(std::string port_name, + int baudrate) : + port_name_(port_name), + baudrate_(baudrate), + data_bits_(8), + parity_bits_('N'), + stop_bits_(1) {} + +SerialDevice::~SerialDevice() { + CloseDevice(); +} + +bool SerialDevice::Init() { + + DLOG_INFO << "Attempting to open device " << port_name_ << " with baudrate " << baudrate_; + if (port_name_.c_str() == nullptr) { + port_name_ = "/dev/ttyUSB0"; + } + if (OpenDevice() && ConfigDevice()) { + FD_ZERO(&serial_fd_set_); + FD_SET(serial_fd_, &serial_fd_set_); + DLOG_INFO << "...Serial started successfully."; + return true; + } else { + DLOG_ERROR << "...Failed to start serial "<. + ***************************************************************************/ + +#ifndef ROBORTS_SDK_SERIAL_DEVICE_H +#define ROBORTS_SDK_SERIAL_DEVICE_H +#include +#include + +#include +#include +#include + +#include "../utilities/log.h" +#include "hardware_interface.h" + +namespace roborts_sdk { +/** + * @brief serial device class inherited from hardware interface + */ +class SerialDevice: public HardwareInterface { + public: + /** + * @brief Constructor of serial device + * @param port_name port name, i.e. /dev/ttyUSB0 + * @param baudrate serial baudrate + */ + SerialDevice(std::string port_name, int baudrate); + /** + * @brief Destructor of serial device to close the device + */ + ~SerialDevice(); + /** + * @brief Initialization of serial device to config and open the device + * @return True if success + */ + virtual bool Init() override ; + /** + * @brief Serial device read function + * @param buf Given buffer to be updated by reading + * @param len Read data length + * @return -1 if failed, else the read length + */ + virtual int Read(uint8_t *buf, int len) override ; + /** + * @brief Write the buffer data into device to send the data + * @param buf Given buffer to be sent + * @param len Send data length + * @return < 0 if failed, else the send length + */ + virtual int Write(const uint8_t *buf, int len) override ; + + private: + /** + * @brief Open the serial device + * @return True if open successfully + */ + bool OpenDevice(); + /** + * @brief Close the serial device + * @return True if close successfully + */ + bool CloseDevice(); + + /** + * @brief Configure the device + * @return True if configure successfully + */ + bool ConfigDevice(); + + //! port name of the serial device + std::string port_name_; + //! baudrate of the serial device + int baudrate_; + //! stop bits of the serial device, as default + int stop_bits_; + //! data bits of the serial device, as default + int data_bits_; + //! parity bits of the serial device, as default + char parity_bits_; + //! serial handler + int serial_fd_; + //! set flag of serial handler + fd_set serial_fd_set_; + //! termios config for serial handler + struct termios new_termios_, old_termios_; +}; +} +#endif //ROBORTS_SDK_SERIAL_DEVICE_H diff --git a/roborts_base/roborts_sdk/protocol/protocol.cpp b/roborts_base/roborts_sdk/protocol/protocol.cpp new file mode 100644 index 00000000..c981bbee --- /dev/null +++ b/roborts_base/roborts_sdk/protocol/protocol.cpp @@ -0,0 +1,909 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "protocol.h" + +namespace roborts_sdk { + +Protocol::Protocol(std::shared_ptr serial_device_ptr) : + running_(false), + serial_device_ptr_(serial_device_ptr), seq_num_(0), + is_large_data_protocol_(true), reuse_buffer_(true), + poll_tick_(10) { + +} + +Protocol::~Protocol() { + running_ = false; + if (send_poll_thread_.joinable()) { + send_poll_thread_.join(); + } + if (receive_pool_thread_.joinable()) { + receive_pool_thread_.join(); + } + if (recv_stream_ptr_) { + delete[] recv_stream_ptr_->recv_buff; + delete recv_stream_ptr_; + } + if (recv_buff_ptr_) { + delete[]recv_buff_ptr_; + } + if (recv_container_ptr_) { + delete recv_container_ptr_; + } +} + +bool Protocol::Init() { + + seq_num_ = 0; + auto max_buffer_size = BUFFER_SIZE; + auto max_pack_size = MAX_PACK_SIZE; + auto session_table_num = SESSION_TABLE_NUM; + memory_pool_ptr_ = std::make_shared(max_buffer_size, + max_pack_size, + session_table_num); + memory_pool_ptr_->Init(); + + recv_buff_ptr_ = new uint8_t[BUFFER_SIZE]; + + recv_stream_ptr_ = new RecvStream(); + recv_stream_ptr_->recv_buff = new uint8_t[MAX_PACK_SIZE]; + recv_stream_ptr_->recv_index = 0; + recv_stream_ptr_->reuse_index = 0; + recv_stream_ptr_->reuse_count = 0; + + recv_container_ptr_ = new RecvContainer(); + + SetupSession(); + + running_ = true; + send_poll_thread_ = std::thread(&Protocol::AutoRepeatSendCheck, this); + receive_pool_thread_ = std::thread(&Protocol::ReceivePool, this); + return true; +} + +void Protocol::AutoRepeatSendCheck() { + while (running_) { + unsigned int i; + + std::chrono::steady_clock::time_point current_time_stamp; + + for (i = 1; i < SESSION_TABLE_NUM; i++) { + + if (cmd_session_table_[i].usage_flag == 1) { + current_time_stamp = std::chrono::steady_clock::now(); + if ((std::chrono::duration_cast + (current_time_stamp - cmd_session_table_[i].pre_time_stamp) > + cmd_session_table_[i].ack_timeout)) { + + memory_pool_ptr_->LockMemory(); + if (cmd_session_table_[i].retry_time > 0) { + + if (cmd_session_table_[i].sent >= cmd_session_table_[i].retry_time) { + DLOG_ERROR << "Sending timeout, Free session " + << static_cast(cmd_session_table_[i].session_id); + FreeCMDSession(&cmd_session_table_[i]); + } else { + DLOG_ERROR << "Retry session " + << static_cast(cmd_session_table_[i].session_id); + DeviceSend(cmd_session_table_[i].memory_block_ptr->memory_ptr); + cmd_session_table_[i].pre_time_stamp = current_time_stamp; + cmd_session_table_[i].sent++; + } + } else { + DLOG_ERROR << "Send once " << i; + DeviceSend(cmd_session_table_[i].memory_block_ptr->memory_ptr); + cmd_session_table_[i].pre_time_stamp = current_time_stamp; + } + memory_pool_ptr_->UnlockMemory(); + } else { +// DLOG_INFO<<"Wait for timeout Session: "<< i; + } + } + } + usleep(1000); + } +} + +void Protocol::ReceivePool() { + while (running_) { + + RecvContainer *container_ptr = Receive(); + if (container_ptr) { + if (buffer_pool_map_.count(std::make_pair(container_ptr->command_info.cmd_set, + container_ptr->command_info.cmd_id)) == 0) { + buffer_pool_map_[std::make_pair(container_ptr->command_info.cmd_set, + container_ptr->command_info.cmd_id)] + = std::make_shared>(100); + + } + //1 time copy + buffer_pool_map_[std::make_pair(container_ptr->command_info.cmd_set, + container_ptr->command_info.cmd_id)]->Push(*container_ptr); + } + usleep(100); + } +} + +bool Protocol::Take(const CommandInfo *command_info, + MessageHeader *message_header, + void *message_data) { + if (buffer_pool_map_.count(std::make_pair(command_info->cmd_set, + command_info->cmd_id)) == 0) { +// DLOG_ERROR<<"take failed"; + return false; + } else { + //1 time copy + RecvContainer container; + if (!buffer_pool_map_[std::make_pair(command_info->cmd_set, + command_info->cmd_id)]->Pop(container)) { +// DLOG_EVERY_N(ERROR, 100)<<"nothing to take"; + return false; + } + + if (memcmp(command_info, &(container.command_info), sizeof(command_info)) != 0) { + DLOG_ERROR << "Take command does not match the command received"; + DLOG_INFO << int(container.command_info.receiver) << " " << int(command_info->receiver); + return false; + } + //1 time copy + memcpy(message_header, &(container.message_header), sizeof(message_header)); + memcpy(message_data, &(container.message_data), command_info->length); + + return true; + } +} +bool Protocol::SendResponse(const CommandInfo *command_info, + const MessageHeader *message_header, + void *message_data) { + return SendACK(message_header->session_id, + message_header->seq_num, + command_info->receiver, + message_data, command_info->length); +} +bool Protocol::SendRequest(const CommandInfo *command_info, + void *message_data) { + return SendCMD(command_info->cmd_set, command_info->cmd_id, + command_info->receiver, message_data, command_info->length, + CMDSessionMode::CMD_SESSION_AUTO); +} +bool Protocol::SendMessage(const CommandInfo *command_info, + void *message_data) { + return SendCMD(command_info->cmd_set, command_info->cmd_id, + command_info->receiver, message_data, command_info->length, + CMDSessionMode::CMD_SESSION_0); +} + +/*************************** Session Management **************************/ +void Protocol::SetupSession() { + uint8_t i, j; + for (i = 0; i < SESSION_TABLE_NUM; i++) { + cmd_session_table_[i].session_id = i; + cmd_session_table_[i].usage_flag = false; + cmd_session_table_[i].memory_block_ptr = nullptr; + } + + for (i = 0; i < RECEIVER_NUM; i++) { + for (j = 0; j < (SESSION_TABLE_NUM - 1); j++) { + ack_session_table_[i][j].session_id = j + 1; + ack_session_table_[i][j].session_status = ACKSessionStatus::ACK_SESSION_IDLE; + ack_session_table_[i][j].memory_block_ptr = nullptr; + } + } +} + +CMDSession *Protocol::AllocCMDSession(CMDSessionMode session_mode, uint16_t size) { + uint32_t i; + MemoryBlock *memory_block_ptr = nullptr; + + if (session_mode == CMDSessionMode::CMD_SESSION_0 || session_mode == CMDSessionMode::CMD_SESSION_1) { + if (cmd_session_table_[(uint16_t) session_mode].usage_flag == 0) { + i = static_cast(session_mode); + } else { + DLOG_ERROR << "session " << static_cast(session_mode) << " is busy\n"; + return nullptr; + } + } else { + for (i = 2; i < SESSION_TABLE_NUM; i++) { + if (cmd_session_table_[i].usage_flag == 0) { + break; + } + } + + } + + if (i < 32 && cmd_session_table_[i].usage_flag == 0) { + + cmd_session_table_[i].usage_flag = 1; + memory_block_ptr = memory_pool_ptr_->AllocMemory(size); + if (memory_block_ptr == nullptr) { + cmd_session_table_[i].usage_flag = 0; + } else { +// DLOG_INFO<<"find "<usage_flag == 1) { + memory_pool_ptr_->FreeMemory(session_ptr->memory_block_ptr); + session_ptr->usage_flag = 0; + } +} + +ACKSession *Protocol::AllocACKSession(uint8_t receiver, uint16_t session_id, uint16_t size) { + MemoryBlock *memory_block_ptr = nullptr; + if (session_id > 0 && session_id < 32) { + if (ack_session_table_[receiver][session_id - 1].memory_block_ptr) { + FreeACKSession(&ack_session_table_[receiver][session_id - 1]); + } + + memory_block_ptr = memory_pool_ptr_->AllocMemory(size); + if (memory_block_ptr == nullptr) { + DLOG_ERROR << "there is not enough memory"; + return nullptr; + } else { + ack_session_table_[receiver][session_id - 1].memory_block_ptr = memory_block_ptr; + return &ack_session_table_[receiver][session_id - 1]; + } + } + return nullptr; +} + +void Protocol::FreeACKSession(ACKSession *session_ptr) { + memory_pool_ptr_->FreeMemory(session_ptr->memory_block_ptr); +} + +/****************************** Send Pipline *****************************/ +bool Protocol::SendCMD(uint8_t cmd_set, uint8_t cmd_id, uint8_t receiver, + void *data_ptr, uint16_t data_length, + CMDSessionMode session_mode, + std::chrono::milliseconds ack_timeout, int retry_time) { + + CMDSession *cmd_session_ptr = nullptr; + Header *header_ptr = nullptr; + uint8_t cmd_set_prefix[] = {cmd_id, cmd_set}; + uint32_t crc_data; + + uint16_t pack_length = 0; + + + //calculate pack_length first + if (data_length == 0 || data_ptr == nullptr) { + DLOG_ERROR << "No data send."; + return false; + } + pack_length = HEADER_LEN + + CMD_SET_PREFIX_LEN + + data_length + CRC_DATA_LEN; + + //second get the param into the session + switch (session_mode) { + + case CMDSessionMode::CMD_SESSION_0: + //lock + memory_pool_ptr_->LockMemory(); + cmd_session_ptr = AllocCMDSession(CMDSessionMode::CMD_SESSION_0, pack_length); + + if (cmd_session_ptr == nullptr) { + //unlock + memory_pool_ptr_->UnlockMemory(); + DLOG_ERROR << "Allocate CMD session failed."; + return false; + } + + //pack into cmd_session memory_block + header_ptr = (Header *) cmd_session_ptr->memory_block_ptr->memory_ptr; + header_ptr->sof = SOF; + header_ptr->length = pack_length; + header_ptr->version = VERSION; + header_ptr->session_id = cmd_session_ptr->session_id; + header_ptr->is_ack = 0; + header_ptr->reserved0 = 0; + header_ptr->sender = DEVICE; + header_ptr->receiver = receiver; + header_ptr->reserved1 = 0; + header_ptr->seq_num = seq_num_; + header_ptr->crc = CRC16Calc(cmd_session_ptr->memory_block_ptr->memory_ptr, HEADER_LEN - CRC_HEAD_LEN); + + // pack the cmd prefix ,data and data crc into memory block one by one + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN, cmd_set_prefix, CMD_SET_PREFIX_LEN); + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN + CMD_SET_PREFIX_LEN, data_ptr, data_length); + + crc_data = CRC32Calc(cmd_session_ptr->memory_block_ptr->memory_ptr, pack_length - CRC_DATA_LEN); + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + pack_length - CRC_DATA_LEN, &crc_data, CRC_DATA_LEN); + + // send it using device + DeviceSend(cmd_session_ptr->memory_block_ptr->memory_ptr); + seq_num_++; + FreeCMDSession(cmd_session_ptr); + //unlock + memory_pool_ptr_->UnlockMemory(); + break; + + case CMDSessionMode::CMD_SESSION_1: + //lock + memory_pool_ptr_->LockMemory(); + cmd_session_ptr = AllocCMDSession(CMDSessionMode::CMD_SESSION_1, pack_length); + + if (cmd_session_ptr == nullptr) { + //unlock + memory_pool_ptr_->UnlockMemory(); + DLOG_ERROR << "Allocate CMD session failed."; + return false; + } + + //may be used more than once, seq_num_ should increase if duplicated. + if (seq_num_ == cmd_session_ptr->pre_seq_num) { + seq_num_++; + } + //pack into cmd_session memory_block + header_ptr = (Header *) cmd_session_ptr->memory_block_ptr->memory_ptr; + header_ptr->sof = SOF; + header_ptr->length = pack_length; + header_ptr->version = VERSION; + header_ptr->session_id = cmd_session_ptr->session_id; + header_ptr->is_ack = 0; + header_ptr->reserved0 = 0; + header_ptr->sender = DEVICE; + header_ptr->receiver = receiver; + header_ptr->reserved1 = 0; + header_ptr->seq_num = seq_num_; + header_ptr->crc = CRC16Calc(cmd_session_ptr->memory_block_ptr->memory_ptr, HEADER_LEN - CRC_HEAD_LEN); + + // pack the cmd prefix ,data and data crc into memory block one by one + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN, cmd_set_prefix, CMD_SET_PREFIX_LEN); + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN + CMD_SET_PREFIX_LEN, data_ptr, data_length); + + crc_data = CRC32Calc(cmd_session_ptr->memory_block_ptr->memory_ptr, pack_length - CRC_DATA_LEN); + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + pack_length - CRC_DATA_LEN, &crc_data, CRC_DATA_LEN); + + // seem useless + cmd_session_ptr->cmd_id = cmd_id; + cmd_session_ptr->cmd_set = cmd_set; + cmd_session_ptr->pre_seq_num = seq_num_++; + + cmd_session_ptr->ack_timeout = (ack_timeout > poll_tick_) ? ack_timeout : poll_tick_; + cmd_session_ptr->pre_time_stamp = std::chrono::steady_clock::now(); + cmd_session_ptr->sent = 1; + cmd_session_ptr->retry_time = 1; + // send it using device + DeviceSend(cmd_session_ptr->memory_block_ptr->memory_ptr); + //unlock + memory_pool_ptr_->UnlockMemory(); + break; + + case CMDSessionMode::CMD_SESSION_AUTO: + //lock + memory_pool_ptr_->LockMemory(); + cmd_session_ptr = AllocCMDSession(CMDSessionMode::CMD_SESSION_AUTO, pack_length); + + if (cmd_session_ptr == nullptr) { + //unlock + memory_pool_ptr_->UnlockMemory(); + DLOG_ERROR << "Allocate CMD session failed."; + return false; + } + + //may be used more than once, seq_num_ should increase if duplicated. + if (seq_num_ == cmd_session_ptr->pre_seq_num) { + seq_num_++; + } + //pack into cmd_session memory_block + header_ptr = (Header *) cmd_session_ptr->memory_block_ptr->memory_ptr; + header_ptr->sof = SOF; + header_ptr->length = pack_length; + header_ptr->version = VERSION; + header_ptr->session_id = cmd_session_ptr->session_id; + header_ptr->is_ack = 0; + header_ptr->reserved0 = 0; + header_ptr->sender = DEVICE; + header_ptr->receiver = receiver; + header_ptr->reserved1 = 0; + header_ptr->seq_num = seq_num_; + header_ptr->crc = CRC16Calc(cmd_session_ptr->memory_block_ptr->memory_ptr, HEADER_LEN - CRC_HEAD_LEN); + + // pack the cmd prefix ,data and data crc into memory block one by one + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN, cmd_set_prefix, CMD_SET_PREFIX_LEN); + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN + CMD_SET_PREFIX_LEN, data_ptr, data_length); + + crc_data = CRC32Calc(cmd_session_ptr->memory_block_ptr->memory_ptr, pack_length - CRC_DATA_LEN); + memcpy(cmd_session_ptr->memory_block_ptr->memory_ptr + pack_length - CRC_DATA_LEN, &crc_data, CRC_DATA_LEN); + + // seem useless + cmd_session_ptr->cmd_id = cmd_id; + cmd_session_ptr->cmd_set = cmd_set; + cmd_session_ptr->pre_seq_num = seq_num_++; + + cmd_session_ptr->ack_timeout = (ack_timeout > poll_tick_) ? ack_timeout : poll_tick_; + cmd_session_ptr->pre_time_stamp = std::chrono::steady_clock::now(); + cmd_session_ptr->sent = 1; + cmd_session_ptr->retry_time = retry_time; + // send it using device + DeviceSend(cmd_session_ptr->memory_block_ptr->memory_ptr); + //unlock + memory_pool_ptr_->UnlockMemory(); + break; + + default:DLOG_ERROR << "session mode is not valid"; + return false; + } + + return true; + +} + +bool Protocol::SendACK(uint8_t session_id, uint16_t seq_num, uint8_t receiver, + void *ack_ptr, uint16_t ack_length) { + ACKSession *ack_session_ptr = nullptr; + Header *header_ptr = nullptr; + uint32_t crc_data = 0; + uint16_t pack_length = 0; + + if (ack_ptr == nullptr || ack_length == 0) { + pack_length = HEADER_LEN; + } else { + pack_length = HEADER_LEN + + ack_length + CRC_DATA_LEN; + } + + if (session_id == 0 || session_id > 31) { + DLOG_ERROR << ("ack session id should be from 1 to 31."); + return false; + } else { + + //lock + memory_pool_ptr_->LockMemory(); + ack_session_ptr = AllocACKSession(receiver, session_id, pack_length); + + if (ack_session_ptr == nullptr) { + //unlock + memory_pool_ptr_->UnlockMemory(); + DLOG_ERROR << "Allocate ACK session failed."; + return false; + } + + //pack into ack_session memory_block + header_ptr = (Header *) ack_session_ptr->memory_block_ptr->memory_ptr; + header_ptr->sof = SOF; + header_ptr->length = pack_length; + header_ptr->version = VERSION; + header_ptr->session_id = ack_session_ptr->session_id; + header_ptr->is_ack = 1; + header_ptr->reserved0 = 0; + header_ptr->sender = DEVICE; + header_ptr->receiver = receiver; + header_ptr->reserved1 = 0; + header_ptr->seq_num = seq_num; + header_ptr->crc = CRC16Calc(ack_session_ptr->memory_block_ptr->memory_ptr, HEADER_LEN - CRC_HEAD_LEN); + + // pack the cmd prefix ,data and data crc into memory block one by one + if (ack_ptr != nullptr && ack_length != 0) { + memcpy(ack_session_ptr->memory_block_ptr->memory_ptr + HEADER_LEN, ack_ptr, ack_length); + crc_data = CRC32Calc(ack_session_ptr->memory_block_ptr->memory_ptr, pack_length - CRC_DATA_LEN); + memcpy(ack_session_ptr->memory_block_ptr->memory_ptr + pack_length - CRC_DATA_LEN, &crc_data, CRC_DATA_LEN); + } + + // send it using device + DeviceSend(ack_session_ptr->memory_block_ptr->memory_ptr); + ack_session_table_[receiver][session_id - 1].session_status = ACKSessionStatus::ACK_SESSION_USING; + //unlock + memory_pool_ptr_->UnlockMemory(); + return true; + } + +} + +bool Protocol::DeviceSend(uint8_t *buf) { + int ans; + Header *header_ptr = (Header *) buf; + +// For debug and visualzation: +// ans = header_ptr->length; +// for(int i =0;ilength;i++){ +// printf("send_byte %d:\t %X\n ", i, buf[i]); +// } +// std::cout<<"----------------"<Write(buf, header_ptr->length); + + if (ans <= 0) { + DLOG_ERROR << "Port failed."; + } else if (ans != header_ptr->length) { + DLOG_ERROR << "Port send failed, send length:" << ans << "package length" << header_ptr->length; + } else { + DLOG_INFO << "Port send success."; + return true; + } + return false; +} + +/****************************** Recv Pipline ******************************/ +RecvContainer *Protocol::Receive() { + + //! Bool to check if the protocol parser has finished a full frame + bool is_frame = false; + + //! Step 1: Check if the buffer has been consumed + if (recv_buff_read_pos_ >= recv_buff_read_len_) { + recv_buff_read_pos_ = 0; + recv_buff_read_len_ = serial_device_ptr_->Read(recv_buff_ptr_, BUFFER_SIZE); + } + + //! Step 2: + //! For large data protocol, store the value and only verify the header + //! For small data protocol, Go through the buffer and return when you + //! see a full frame. buf_read_pos will maintain state about how much + //! buffer data we have already read + + //TODO: unhandled + if (is_large_data_protocol_ && recv_buff_read_len_ == BUFFER_SIZE) { + + memcpy(recv_stream_ptr_->recv_buff + (recv_stream_ptr_->recv_index), recv_buff_ptr_, + BUFFER_SIZE); + recv_stream_ptr_->recv_index += BUFFER_SIZE; + recv_buff_read_pos_ = BUFFER_SIZE; + } else { + for (recv_buff_read_pos_; recv_buff_read_pos_ < recv_buff_read_len_; + recv_buff_read_pos_++) { + is_frame = ByteHandler(recv_buff_ptr_[recv_buff_read_pos_]); + + if (is_frame) { + return recv_container_ptr_; + } + } + } + + //! Step 3: If we don't find a full frame by this time, return nullptr. + return nullptr; +} + +bool Protocol::ByteHandler(const uint8_t byte) { + recv_stream_ptr_->reuse_count = 0; + recv_stream_ptr_->reuse_index = MAX_PACK_SIZE; + + bool is_frame = StreamHandler(byte); + + if (reuse_buffer_) { + if (recv_stream_ptr_->reuse_count != 0) { + while (recv_stream_ptr_->reuse_index < MAX_PACK_SIZE) { + /*! @note because reuse_index maybe re-located, so reuse_index must + * be + * always point to un-used index + * re-loop the buffered data + * */ + is_frame = StreamHandler(recv_stream_ptr_->recv_buff[recv_stream_ptr_->reuse_index++]); + } + recv_stream_ptr_->reuse_count = 0; + } + } + return is_frame; +} + +bool Protocol::StreamHandler(uint8_t byte) { + + // push the byte into filter buffer + if (recv_stream_ptr_->recv_index < MAX_PACK_SIZE) { + recv_stream_ptr_->recv_buff[recv_stream_ptr_->recv_index] = byte; + recv_stream_ptr_->recv_index++; + } else { + LOG_ERROR << "Buffer overflow"; + memset(recv_stream_ptr_->recv_buff, 0, recv_stream_ptr_->recv_index); + recv_stream_ptr_->recv_index = 0; + } + + bool is_frame = CheckStream(); + return is_frame; +} + +bool Protocol::CheckStream() { + Header *header_ptr = (Header *) (recv_stream_ptr_->recv_buff); + + bool is_frame = false; + if (recv_stream_ptr_->recv_index < HEADER_LEN) { + return false; + } else if (recv_stream_ptr_->recv_index == HEADER_LEN) { + is_frame = VerifyHeader(); + } else if (recv_stream_ptr_->recv_index == header_ptr->length) { + is_frame = VerifyData(); + } + + return is_frame; +} + +bool Protocol::VerifyHeader() { + Header *header_ptr = (Header *) (recv_stream_ptr_->recv_buff); + bool is_frame = false; + + if ((header_ptr->sof == SOF) && (header_ptr->version == VERSION) && + (header_ptr->length < MAX_PACK_SIZE) && (header_ptr->reserved0 == 0) && + (header_ptr->reserved1 == 0) && (header_ptr->receiver == DEVICE || header_ptr->receiver == 0xFF) && + CRCHeadCheck((uint8_t *) header_ptr, HEADER_LEN)) { + // It is an unused part because minimum package is more longer than a header + if (header_ptr->length == HEADER_LEN) { + + is_frame = ContainerHandler(); + //prepare data stream + PrepareStream(); + } + } else { + //shift the data stream + ShiftStream(); + } + + return is_frame; + +} + +bool Protocol::VerifyData() { + Header *header_ptr = (Header *) (recv_stream_ptr_->recv_buff); + bool is_frame = false; + + if (CRCTailCheck((uint8_t *) header_ptr, header_ptr->length)) { + + is_frame = ContainerHandler(); + //prepare data stream + PrepareStream(); + } else { + //reuse the data stream + ReuseStream(); + } + + return is_frame; +} + +bool Protocol::ContainerHandler() { + + Header *session_header_ptr = nullptr; + Header *header_ptr = (Header *) (recv_stream_ptr_->recv_buff); + bool is_frame = false; + + if (header_ptr->is_ack) { + + if (header_ptr->session_id > 0 && header_ptr->session_id < 32) { + if (cmd_session_table_[header_ptr->session_id].usage_flag == 1) { + memory_pool_ptr_->LockMemory(); + session_header_ptr = (Header *) cmd_session_table_[header_ptr->session_id].memory_block_ptr->memory_ptr; + if (session_header_ptr->session_id == header_ptr->session_id && + session_header_ptr->seq_num == header_ptr->seq_num) { + + recv_container_ptr_->message_header.is_ack = true; + recv_container_ptr_->message_header.seq_num = header_ptr->seq_num; + recv_container_ptr_->message_header.session_id = header_ptr->session_id; + recv_container_ptr_->command_info.length = header_ptr->length - HEADER_LEN - CRC_DATA_LEN; + recv_container_ptr_->command_info.sender = header_ptr->sender; + recv_container_ptr_->command_info.receiver = header_ptr->receiver; + recv_container_ptr_->command_info.cmd_set = cmd_session_table_[header_ptr->session_id].cmd_set; + recv_container_ptr_->command_info.cmd_id = cmd_session_table_[header_ptr->session_id].cmd_id; + recv_container_ptr_->command_info.need_ack = true; + + memcpy(recv_container_ptr_->message_data.raw_data, (uint8_t *) header_ptr + HEADER_LEN, + header_ptr->length - HEADER_LEN - CRC_DATA_LEN); + + is_frame = true; + FreeCMDSession(&cmd_session_table_[header_ptr->session_id]); + memory_pool_ptr_->UnlockMemory(); + // TODO: notify mechanism ,notify ack received and get recv container ready + + } else { + memory_pool_ptr_->UnlockMemory(); + } + } + } + } else { + switch (header_ptr->session_id) { + case 0:recv_container_ptr_->message_header.is_ack = false; + recv_container_ptr_->message_header.seq_num = header_ptr->seq_num; + recv_container_ptr_->message_header.session_id = header_ptr->session_id; + recv_container_ptr_->command_info.sender = header_ptr->sender; + recv_container_ptr_->command_info.receiver = header_ptr->receiver; + recv_container_ptr_->command_info.length = header_ptr->length - HEADER_LEN - CMD_SET_PREFIX_LEN - CRC_DATA_LEN; + recv_container_ptr_->command_info.cmd_set = *((uint8_t *) header_ptr + HEADER_LEN + 1); + recv_container_ptr_->command_info.cmd_id = *((uint8_t *) header_ptr + HEADER_LEN); + recv_container_ptr_->command_info.need_ack = false; + + memcpy(recv_container_ptr_->message_data.raw_data, (uint8_t *) header_ptr + HEADER_LEN + CMD_SET_PREFIX_LEN, + header_ptr->length - HEADER_LEN - CMD_SET_PREFIX_LEN - CRC_DATA_LEN); + + is_frame = true; + break; + case 1: + //TODO: Currently regard session_1 as session_auto but never change the ack session status, ack session always stay idle status for session_1 + //break; + default: + if (header_ptr->session_id > 31) { + return false; + } else { + //TODO ack session table is supposed to indexed by command sender instead of receiver, as receiver is always local device address + switch (ack_session_table_[header_ptr->receiver][header_ptr->session_id - 1].session_status) { + case ACKSessionStatus::ACK_SESSION_IDLE: + + if (header_ptr->session_id > 1) { + memory_pool_ptr_->LockMemory(); + ack_session_table_[header_ptr->receiver][header_ptr->session_id - 1].session_status = + ACKSessionStatus::ACK_SESSION_PROCESS; + memory_pool_ptr_->UnlockMemory(); + } + + recv_container_ptr_->message_header.is_ack = false; + recv_container_ptr_->message_header.seq_num = header_ptr->seq_num; + recv_container_ptr_->message_header.session_id = header_ptr->session_id; + recv_container_ptr_->command_info.sender = header_ptr->sender; + recv_container_ptr_->command_info.receiver = header_ptr->receiver; + recv_container_ptr_->command_info.length = + header_ptr->length - HEADER_LEN - CMD_SET_PREFIX_LEN - CRC_DATA_LEN; + recv_container_ptr_->command_info.cmd_set = *((uint8_t *) header_ptr + HEADER_LEN + 1); + recv_container_ptr_->command_info.cmd_id = *((uint8_t *) header_ptr + HEADER_LEN); + recv_container_ptr_->command_info.need_ack = true; + + memcpy(recv_container_ptr_->message_data.raw_data, + (uint8_t *) header_ptr + HEADER_LEN + CMD_SET_PREFIX_LEN, + header_ptr->length - HEADER_LEN - CMD_SET_PREFIX_LEN - CRC_DATA_LEN); + is_frame = true; + break; + + case ACKSessionStatus::ACK_SESSION_PROCESS: + + DLOG_INFO << "Wait for app ack for session " << header_ptr->session_id; + break; + + case ACKSessionStatus::ACK_SESSION_USING: + + memory_pool_ptr_->LockMemory(); + session_header_ptr = (Header *) ack_session_table_[header_ptr->receiver][header_ptr->session_id + - 1].memory_block_ptr->memory_ptr; + + if (session_header_ptr->seq_num == header_ptr->seq_num) { + DeviceSend((uint8_t *) session_header_ptr); + memory_pool_ptr_->UnlockMemory(); + } else { + + ack_session_table_[header_ptr->receiver][header_ptr->session_id - 1].session_status = + ACKSessionStatus::ACK_SESSION_PROCESS; + memory_pool_ptr_->UnlockMemory(); + + recv_container_ptr_->message_header.is_ack = false; + recv_container_ptr_->message_header.seq_num = header_ptr->seq_num; + recv_container_ptr_->message_header.session_id = header_ptr->session_id; + recv_container_ptr_->command_info.sender = header_ptr->sender; + recv_container_ptr_->command_info.receiver = header_ptr->receiver; + recv_container_ptr_->command_info.length = + header_ptr->length - HEADER_LEN - CMD_SET_PREFIX_LEN - CRC_DATA_LEN; + recv_container_ptr_->command_info.cmd_set = *((uint8_t *) header_ptr + HEADER_LEN + 1); + recv_container_ptr_->command_info.cmd_id = *((uint8_t *) header_ptr + HEADER_LEN); + recv_container_ptr_->command_info.need_ack = true; + memcpy(recv_container_ptr_->message_data.raw_data, + (uint8_t *) header_ptr + HEADER_LEN + CMD_SET_PREFIX_LEN, + header_ptr->length - HEADER_LEN - CMD_SET_PREFIX_LEN - CRC_DATA_LEN); + is_frame = true; + + } + break; + + default: + DLOG_ERROR << "Wrong ACK session status which is out of 0-2"; + } + + } + + } + } + + return is_frame; +} + +/*************************** Stream Management*****************************/ +void Protocol::PrepareStream() { + uint32_t bytes_to_move = HEADER_LEN - 1; + uint32_t index_of_move = recv_stream_ptr_->recv_index - bytes_to_move; + + memmove(recv_stream_ptr_->recv_buff, recv_stream_ptr_->recv_buff + index_of_move, bytes_to_move); + memset(recv_stream_ptr_->recv_buff + bytes_to_move, 0, index_of_move); + recv_stream_ptr_->recv_index = bytes_to_move; +} + +void Protocol::ShiftStream() { + if (recv_stream_ptr_->recv_index) { + recv_stream_ptr_->recv_index--; + if (recv_stream_ptr_->recv_index) { + memmove(recv_stream_ptr_->recv_buff, recv_stream_ptr_->recv_buff + 1, recv_stream_ptr_->recv_index); + } + } +} + +void Protocol::ReuseStream() { + uint8_t *buff_ptr = recv_stream_ptr_->recv_buff; + uint16_t bytes_to_move = recv_stream_ptr_->recv_index - HEADER_LEN; + uint8_t *src_ptr = buff_ptr + HEADER_LEN; + + uint16_t n_dest_index = recv_stream_ptr_->reuse_index - bytes_to_move; + uint8_t *dest_ptr = buff_ptr + n_dest_index; + + memmove(dest_ptr, src_ptr, bytes_to_move); + + recv_stream_ptr_->recv_index = HEADER_LEN; + ShiftStream(); + + recv_stream_ptr_->recv_index = n_dest_index; + recv_stream_ptr_->reuse_count++; +} + +/*************************** CRC Calculationns ****************************/ +uint16_t Protocol::CRC16Update(uint16_t crc, uint8_t ch) { + uint16_t tmp; + uint16_t msg; + + msg = 0x00ff & static_cast(ch); + tmp = crc ^ msg; + crc = (crc >> 8) ^ crc_tab16[tmp & 0xff]; + + return crc; +} + +uint32_t Protocol::CRC32Update(uint32_t crc, uint8_t ch) { + uint32_t tmp; + uint32_t msg; + + msg = 0x000000ffL & static_cast(ch); + tmp = crc ^ msg; + crc = (crc >> 8) ^ crc_tab32[tmp & 0xff]; + return crc; +} + +uint16_t Protocol::CRC16Calc(const uint8_t *data_ptr, size_t length) { + size_t i; + uint16_t crc = CRC_INIT; + + for (i = 0; i < length; i++) { + crc = CRC16Update(crc, data_ptr[i]); + } + + return crc; +} + +uint32_t Protocol::CRC32Calc(const uint8_t *data_ptr, size_t length) { + size_t i; + uint32_t crc = CRC_INIT; + + for (i = 0; i < length; i++) { + crc = CRC32Update(crc, data_ptr[i]); + } + + return crc; +} + +bool Protocol::CRCHeadCheck(uint8_t *data_ptr, size_t length) { + if (CRC16Calc(data_ptr, length) == 0) { + return true; + } else { + return false; + } +} + +bool Protocol::CRCTailCheck(uint8_t *data_ptr, size_t length) { + if (CRC32Calc(data_ptr, length) == 0) { + return true; + } else { + return false; + } +} +} diff --git a/roborts_base/roborts_sdk/protocol/protocol.h b/roborts_base/roborts_sdk/protocol/protocol.h new file mode 100644 index 00000000..17c24760 --- /dev/null +++ b/roborts_base/roborts_sdk/protocol/protocol.h @@ -0,0 +1,514 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_PROTOCOL_H +#define ROBORTS_SDK_PROTOCOL_H +#include +#include + +#include "../hardware/serial_device.h" +#include "../utilities/memory_pool.h" +#include "../utilities/circular_buffer.h" +#include "../utilities/crc.h" +#include +#include +#include + +namespace roborts_sdk { +/*************************** Package Format **************************/ +/** + * @brief Package header used to resolve package + */ +typedef struct Header { + uint32_t sof : 8; + uint32_t length : 10; + uint32_t version : 6; + uint32_t session_id : 5; + uint32_t is_ack : 1; + uint32_t reserved0 : 2; // Always 0 + uint32_t sender: 8; + uint32_t receiver: 8; + uint32_t reserved1 : 16; + uint32_t seq_num : 16; + uint32_t crc : 16; +} Header; + +/*************************** Package Infomation **********************/ +/** + * @brief Key information that a protocol command should include, + * @details Used as an interface with dispatch layer. + */ +typedef struct CommandInfo { + //! command set for different module, i.e. gimbal, chassis + uint8_t cmd_set; + //! command id for different commands in the module + uint8_t cmd_id; + bool need_ack; + uint8_t sender; + uint8_t receiver; + uint16_t length; +} CommandInfo; + +/** + * @brief Message header + * @details Used as an interface with dispatch layer. + */ +typedef struct MessageHeader { + uint16_t seq_num; + uint8_t session_id; + bool is_ack; +} MessageHeader; + +/** + * @brief Message data + * @details Used as an interface with dispatch layer. + */ +typedef union MessageData { + uint8_t raw_data[1024]; +} MessageData; + +/************************* Session Information ***********************/ +/** + * @brief Status for ack session + */ +enum class ACKSessionStatus : uint8_t { + ACK_SESSION_IDLE = 0, /// serial_device_ptr); + /** + * @brief Destructor for protocol + */ + ~Protocol(); + /***************************** Interface ****************************/ + /** + * @brief Initialize memory pool, stream, container and session, + * start the automatic repeat sending thread and receiving pool thread + * @return True if success + */ + bool Init(); + /** + * @brief Check whether the sent command with need for ack gets its ack back and automatic retry sending command + * @details An endless loop to check ack status for the command every timeout duration, + * to resend the command until the sent times reach the given retry times. + */ + void AutoRepeatSendCheck(); + /** + * @brief An endless loop for receiving package and push the package into a circular buffer + * @details 1. Get the package container, + * 2. Check if the pair of command set and id for receiving package exists using a map, + * if not create one circular buffer for this, + * 3. Push the package container to the circular buffer + * Dispath layer is interacted with protocol layer + * by the map from the pair of command set and id to circular buffer. + */ + void ReceivePool(); + /** + * @brief An interface function for dispatch layer to take the message/package from the circular buffer in the protocol layer + * @param command_info Input expected command information + * @param message_header Output message Header + * @param message_data Output message data + * @return True if the message/package is taken successfully from buffer, + * false if the buffer is empty or + * input command information mismatches the information with the same command set and id already in the circular buffer + */ + bool Take(const CommandInfo *command_info, + MessageHeader *message_header, + void *message_data); + /** + * @brief An interface function for dispatch layer to send ack in the protocol layer + * @param command_info Input command information + * @param message_header Input message header + * @param message_data Input message data + * @return true if ack is successfully allocated and sent by protocol layer + */ + bool SendResponse(const CommandInfo *command_info, + const MessageHeader *message_header, + void *message_data); + /** + * @brief An interface function for dispatch layer to send cmd with need for ack in the protocol layer + * @param command_info Input command information + * @param message_data Input message data + * @return true if command is successfully allocated and sent by protocol layer + */ + bool SendRequest(const CommandInfo *command_info, + void *message_data); + /** + * @brief An interface function for dispatch layer to send cmd without need for ack in the protocol layer + * @param command_info Input command information + * @param message_data Input message data + * @return True if command is successfully allocated and sent by protocol layer + */ + bool SendMessage(const CommandInfo *command_info, + void *message_data); + /*************************** Send Pipline ***************************/ + /** + * @brief Assign and send command in the protocol layer + * @param cmd_set Command set for different modules + * @param cmd_id Command id for different commands + * @param receiver Receiver address + * @param data_ptr Pointer for the data head address + * @param data_length Length of data + * @param session_mode Session mode to distinguish whether the command need for ack + * @param ack_timeout Timeout duration to check ack status in AutoRepeatSendCheck. Invalid if no need for ack + * @param retry_time Retry time given to retry sending command in AutoRepeatSendCheck. Invalid if no need for ack + * @return True if command is successfully allocated and sent + */ + bool SendCMD(uint8_t cmd_set, uint8_t cmd_id, uint8_t receiver, + void *data_ptr, uint16_t data_length, + CMDSessionMode session_mode, + std::chrono::milliseconds ack_timeout = std::chrono::milliseconds(50), int retry_time = 5); + /** + * @brief Assign and send ack in the protocol layer + * @param session_id Session id for allocate the ack session corresponding to the command session id + * @param seq_num sequence number, same with the command package + * @param receiver Receiver address, same with the command package sender + * @param ack_ptr Pointer for the ack data head address + * @param ack_length Length of ack data + * @return True if ack is successfully allocated and sent + */ + bool SendACK(uint8_t session_id, uint16_t seq_num, uint8_t receiver, + void *ack_ptr, uint16_t ack_length); + /** + * @brief Use hardware interface in the hardware layer to send the data + * @param buf pointer for the buffer head + * @return True if the buffer is successfully sent, blocked to retry connection + * if the hardware device is disconnected after connection. + */ + bool DeviceSend(uint8_t *buf); + + /*************************** Recv Pipline ***************************/ + /** + * @brief Get the byte from the device receive interface into the receive buffer and put it into byte handler, return if a full frame is got + * @details Receive process consists of following process + * 1. Get the Byte from the device read + * 2. Push it into the stream + * 3. Verify the stream for the header first.If validated, + * then verify the whole package when stream length equal header->length + * 4. If whole package is validated, then get the stream into container handler for package resolving + * to get the receiving container in terms of ack and command + * 5. After whole package resolving, prepare the stream for the next loop, + * 6. Return the container if resolving successfully + * @return Receive Container + */ + RecvContainer *Receive(); + /** + * @brief Input the byte and put it into stream handler with reuse mechanism + * @param byte Input byte from Receive() + * @return True if a full frame is got + */ + bool ByteHandler(const uint8_t byte); + /** + * @brief Input the byte from the byte handler, put it into the reuse stream and check if the stream valid for a frame + * @param byte Input byte from ByteHandler() + * @return True if a full frame is got + */ + bool StreamHandler(uint8_t byte); + /** + * @brief Check the stream to verify the header and data for the certain length + * @return True if a full frame is got + */ + bool CheckStream(); + /** + * @brief Verify if it is a header. + * @details Validate the sof, version, receiver, length and header crc. If success, just go ahead. If not, shift the stream + * @return True if a full frame is got (Currently always false because there is no header only package) + */ + bool VerifyHeader(); + /** + * @brief Verify if it is a full package. + * @details Validate the data crc for whole package. If success, just put it into conatiner handler and then prepare the stream. + * If not, reuse the stream + * @return True if a full frame is got + */ + bool VerifyData(); + /** + * @brief Resolve the package from the stream, classify the ack and command, and get the container. + * @return True if a full frame is got + */ + bool ContainerHandler(); + + /*************************** Stream Management**********************/ + /** + * @brief Prepare the stream to only keep last (header length - 1) data remaining + * @details Used after resolving the package and Getting the container + */ + void PrepareStream(); + /** + * @brief Shift the stream to throw the oldest byte in the stream + * @details Used after the header is validated failed + */ + void ShiftStream(); + /** + * @brief this function will move the data part to buffer end, + * @details head part will move left + * 1. there no re-use data + * |------------------------------------------| <= cache + * ^ + * reuse_index + * [12345678][ data Part ]--------------------| 1. stream_ptr + * [12345678]---------------------[ data Part ] 2. move data to end + * [2345678]----------------------[ data Part ] 3. forward head + * [2345678]------------[ data need to re-use ] 4. final mem layout + * + * 2. already has re-use data + * |---------------------------------[rev data] <= cache + * ^ + * reuse_index, the data already used + * [12345678][ data Part ]-----------[rev data] 1. stream_ptr + * [12345678]-----------[ data Part ][rev data] 2. move data to end + * [2345678]------------[ data Part ][rev data] 3. forward head + * [2345678]------------[ data need to re-use ] 4. final mem layout + * + * the re-use data will loop later + */ + void ReuseStream(); + /************************ Session Management ***********************/ + /** + * @brief Setup the command and ack session for initialization + */ + void SetupSession(); + /** + * @brief Allocate the command session + * @param session_mode Input session mode + * @param size Input the size + * @return Pointer of the command session + */ + CMDSession *AllocCMDSession(CMDSessionMode session_mode, uint16_t size); + /** + * @brief Free the command session + * @param session Input the pointer of command session to be freed + */ + void FreeCMDSession(CMDSession *session); + /** + * @brief Allocate the ack session + * @param receiver Input the receiver address + * @param session_id Input the session id + * @param size Input the size + * @return Pointer of the ack session + */ + ACKSession *AllocACKSession(uint8_t receiver, uint16_t session_id, uint16_t size); + /** + * @brief Free the ack session + * @param session Input the pointer of ack session to be freed + */ + void FreeACKSession(ACKSession *session); + + /******************* CRC Calculationns ***************************/ + /** + * @brief Update CRC16 + * @param crc Input CRC16 to be updated + * @param ch Input data byte + * @return Updated CRC16 + */ + uint16_t CRC16Update(uint16_t crc, uint8_t ch); + /** + * @brief Update CRC32 + * @param crc Input CRC32 to be updated + * @param ch Input data byte + * @return Updated CRC32 + */ + uint32_t CRC32Update(uint32_t crc, uint8_t ch); + /** + * @brief Calculate CRC16 with input data + * @param data_ptr Input pointer of data head + * @param length Input data length + * @return CRC16 + */ + uint16_t CRC16Calc(const uint8_t *data_ptr, size_t length); + /** + * @brief Calculate CRC32 with input data + * @param data_ptr Input pointer of data head + * @param length Input data length + * @return CRC32 + */ + uint32_t CRC32Calc(const uint8_t *data_ptr, size_t length); + /** + * @brief Check if the calculated header CRC16 is same with CRC16 in the header + * @param data_ptr Input pointer of data head + * @param length Input data length + * @return True if header CRC16 is validated successfully + */ + bool CRCHeadCheck(uint8_t *data_ptr, size_t length); + /** + * @brief Check if the calculated header CRC32 is same with CRC32 in the package tail + * @param data_ptr Input pointer of data head + * @param length Input data length + * @return True if tail CRC32 is validated successfully + */ + bool CRCTailCheck(uint8_t *data_ptr, size_t length); + /******************* Const List ***************************/ + + //! size of receive buffer used to read from hardware device + static const size_t BUFFER_SIZE = 1024; + //! max Size of package + static const size_t MAX_PACK_SIZE = 1024; + //! session number for a sender/receiver + static const size_t SESSION_TABLE_NUM = 32; + //! length of header + static const size_t HEADER_LEN = sizeof(Header); + //! length of CRC16 + static const size_t CRC_HEAD_LEN = sizeof(uint16_t); + //! length of CRC32 + static const size_t CRC_DATA_LEN = sizeof(uint32_t); + //! length of the pair of command id and command set + static const size_t CMD_SET_PREFIX_LEN = 2 * sizeof(uint8_t); + + //! SOF + static const uint8_t SOF = 0xAA; + //! version + static const uint8_t VERSION = 0x00; + //! local device address + static const uint8_t DEVICE = 0x00; + //! max number of receiver address + static const uint8_t RECEIVER_NUM = 6; + + private: + //! shared pointer of serial device + std::shared_ptr serial_device_ptr_; + //! shared pointer of memory pool + std::shared_ptr memory_pool_ptr_; + + //! sequence number + uint16_t seq_num_; + //! minimum of ack timeout according to the transmission delay between two devices + std::chrono::milliseconds poll_tick_; + + //! command session table + CMDSession cmd_session_table_[SESSION_TABLE_NUM]; + //! ack session table + ACKSession ack_session_table_[RECEIVER_NUM][SESSION_TABLE_NUM - 1]; + + //! pointer of receive buffer + uint8_t *recv_buff_ptr_; + //! read position in the receive buff + uint16_t recv_buff_read_pos_; + //! length of data read in the receive buffer + uint16_t recv_buff_read_len_; + + //! whether or not support large data that is larger than length of receive buffer + bool is_large_data_protocol_; + //! reuse buffer + bool reuse_buffer_; + + //! pointer of receive stream + RecvStream *recv_stream_ptr_; + //! pointer of receive container + RecvContainer *recv_container_ptr_; + + //! map from the pair of command set and id, to the circular buffer of receive container + std::map, std::shared_ptr>> buffer_pool_map_; + //! if receive pool should run + std::atomic running_; + + //! automatic repeat send thread + std::thread send_poll_thread_; + //! receive pool thread + std::thread receive_pool_thread_; +}; +} +#endif //ROBORTS_SDK_PROTOCOL_H diff --git a/roborts_base/roborts_sdk/protocol/protocol_define.h b/roborts_base/roborts_sdk/protocol/protocol_define.h new file mode 100644 index 00000000..74660964 --- /dev/null +++ b/roborts_base/roborts_sdk/protocol/protocol_define.h @@ -0,0 +1,192 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_SDK_PROTOCOL_DEFINE_H +#define ROBORTS_SDK_PROTOCOL_DEFINE_H + +namespace roborts_sdk { +//DEVICE_ADDRESS +#define MANIFOLD2_ADDRESS (0x00u) +#define CHASSIS_ADDRESS (0X01u) +#define GIMBAL_ADDRESS (0X02u) +#define BROADCAST_ADDRESS (0Xffu) + +//CMD_SET +#define UNIVERSAL_CMD_SET (0x00u) +#define REFEREE_CMD_SET (0x01u) +#define CHASSIS_CMD_SET (0x02u) +#define GIMBAL_CMD_SET (0x03u) +#define TEST_CMD_SET (0xFFu) + +/*----------------------------UNIVERSAL_CMD------------------------------*/ +#define OPEN_SDK_MODE (0X00u) +#define GET_MODULE_STATUS (0X01u) +#define PUSH_STATUS_INFO (0X02u) +#define GET_CAN_BANDWIDTH (0X03u) + +/*-----------------------------REFEREE_CMD-------------------------------*/ +#define GET_REFEREE_VERSION (0X00u) +#define GET_GAME_CONFIG (0X01u) +#define SUBSCRIBE_REFEREE_INFO (0X02u) +#define PUSH_REFEREE_INFO (0X03u) + + +/*-----------------------------CHASSIS_CMD-------------------------------*/ + +#define GET_CHASSIS_VERSION (0x00u) +#define SET_CHASSIS_PARAM (0x01u) +#define GET_CHASSIS_PARAM (0X02u) +typedef struct { + uint16_t wheel_perimeter; + uint16_t wheel_track; + uint16_t wheel_base; + int16_t gimbal_x_offset; + int16_t gimbal_y_offset; +} p_chassis_param_t; +#define SUBSCRIBE_CHASSIS_INFO (0X03u) +#define PUSH_CHASSIS_INFO (0X04u) +typedef struct { + uint8_t mode; + float yaw_motor_angle; + float gyro_angle; + float gyro_palstance; + + int32_t position_x_mm; + int32_t position_y_mm; + int32_t angle_deg; + int16_t v_x_mm; + int16_t v_y_mm; + int16_t palstance_deg; +} p_chassis_info_t; + +#define SET_CHASSIS_MODE (0X05u) +typedef enum { + FOLLOW_GIMBAL, + SEPARATE_GIMBAL, + SEARCH_MODE, + C_MODE_MAX_NUM, +} chassis_mode_e; + +#define CTRL_CHASSIS_SPEED (0X06u) +typedef struct { + float vx; + float vy; + float vw; + int16_t rotate_x_offset; + int16_t rotate_y_offset; + uint16_t res; +} p_chassis_speed_t; + +#define PUSH_UWB_INFO (0X07u) +typedef struct { + int16_t x; + int16_t y; + uint16_t yaw; + int16_t distance[6]; + uint16_t error; + uint16_t res; +} p_uwb_data_t; + +#define CTRL_CHASSIS_SPEED_ACC (0X08u) +typedef struct { + float vx; + float vy; + float vw; + + float ax; + float ay; + float wz; +} p_chassis_spd_acc_t; + +/*-----------------------------GIMBAL_CMD-------------------------------*/ + +#define GET_GIMBAL_VERSION (0x00u) +#define SET_GIMBAL_PARAM (0x01u) +#define GET_GIMBAL_PARAM (0X02u) +#define SUBSCRIBE_GIMBAL_INFO (0X03u) +#define PUSH_GIMBAL_INFO (0X04u) +typedef struct { + uint8_t mode; + uint16_t fric_spd; + uint32_t shoot_num; + + float pit_relative_angle; + float yaw_relative_angle; + float pit_gyro_angle; + float yaw_gyro_angle; + + float yaw_palstance; + float pit_palstance; +} p_gimbal_info_t; + +#define ADJUST_GIMBAL_CENTER (0X05u) +#define SET_GIMBAL_MODE (0X06u) +typedef enum { + GYRO_CONTROL, + CODE_CONTROL, + G_MODE_MAX_NUM, +} gimbal_mode_e; + +#define CTRL_GIMBAL_RATE (0X07u) +typedef struct { + float pit_rate; + uint16_t pit_time; + float yaw_rate; + uint16_t yaw_time; +} p_gimbal_speed_t; + +//#define CTRL_GIMBAL_ANGLE (0X08u) +//typedef struct { +// float pit; +// float yaw; +//} p_gimbal_angle_t; + +#define CTRL_FRIC_WHEEL_SPEED (0X09u) +typedef uint16_t p_fric_wheel_speed_t; + +#define CTRL_GIMBAL_SHOOT (0x0Au) +typedef enum { + SHOOT_STOP = 0, + SHOOT_ONCE, + SHOOT_CONTINUOUS, +} shoot_cmd_e; + +typedef struct { + uint8_t shoot_cmd; + uint32_t shoot_add_num; + uint16_t shoot_freq; +} p_gimbal_shoot_t; + +#define CTRL_GIMBAL_ANGLE (0x0Bu) +typedef struct{ + union{ + uint8_t flag; + struct { + uint8_t yaw_mode: 1;//0 means absolute, 1 means relative; + uint8_t pitch_mode: 1; + } bit; + } ctrl; + float pit_absolute; + float pit_relative; + float yaw_absolute; + float yaw_relative; +}p_gimbal_angle_t; + +//TEST_CMD +#define TEXT_ECHO_TRANSMIT (0x00u) + +} +#endif //ROBORTS_SDK_PROTOCOL_DEFINE_H diff --git a/roborts_base/roborts_sdk/sdk.h b/roborts_base/roborts_sdk/sdk.h new file mode 100644 index 00000000..cacb780e --- /dev/null +++ b/roborts_base/roborts_sdk/sdk.h @@ -0,0 +1,26 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_H +#define ROBORTS_SDK_H + +#include "dispatch/handle.h" +#include "dispatch/execution.h" +#include "dispatch/dispatch.h" +#include "protocol/protocol_define.h" + +#endif //ROBORTS_SDK_H diff --git a/roborts_base/roborts_sdk/test/sdk_test.cpp b/roborts_base/roborts_sdk/test/sdk_test.cpp new file mode 100644 index 00000000..fec2b286 --- /dev/null +++ b/roborts_base/roborts_sdk/test/sdk_test.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include + +#include "../sdk.h" +#include "../protocol/protocol_define.h" + +int main() { + auto h=std::make_shared(); + int count; + + /*-----------Subscriber Test-------------*/ + auto func = [&count] (const std::shared_ptr message) -> void{ + DLOG_INFO<<"chassis_msg_"<position_x_mm); + count++; + }; + auto func2 = [&count] (const std::shared_ptr message) -> void{ + DLOG_INFO<<"chassis_msg_"<error; + count++; + }; + auto func3 = [&count] (const std::shared_ptr message) -> void{ + DLOG_INFO<<"chassis_msg_"<pit_relative_angle; + count++; + }; + auto sub1=h->CreateSubscriber(CHASSIS_CMD_SET,PUSH_CHASSIS_INFO,CHASSIS_ADDRESS,MANIFOLD2_ADDRESS,func); + auto sub2=h->CreateSubscriber(CHASSIS_CMD_SET,PUSH_UWB_INFO,CHASSIS_ADDRESS,MANIFOLD2_ADDRESS,func2); + auto sub3=h->CreateSubscriber(GIMBAL_CMD_SET,PUSH_GIMBAL_INFO,GIMBAL_ADDRESS,BROADCAST_ADDRESS,func3); + + /*-----------Publisher Test-------------*/ + auto pub1 = h->CreatePublisher(CHASSIS_CMD_SET,CTRL_CHASSIS_SPEED,MANIFOLD2_ADDRESS,CHASSIS_ADDRESS); + p_chassis_speed_t chassis_speed; + chassis_speed.rotate_x_offset=0; + chassis_speed.rotate_y_offset=0; + chassis_speed.vx=100; + chassis_speed.vy=0; + chassis_speed.vw=0; + chassis_speed.res=0; + pub1->Publish(chassis_speed); + + /*-----------Client Test-------------*/ + auto client1=h->CreateClient(CHASSIS_CMD_SET,SET_CHASSIS_MODE,MANIFOLD2_ADDRESS,CHASSIS_ADDRESS); + auto mode = std::make_shared(SEPARATE_GIMBAL); + + client1->AsyncSendRequest(mode,[](Client::SharedFuture){ + std::cout<<"get!"<Spin(); + usleep(10); + } + + +} \ No newline at end of file diff --git a/roborts_base/roborts_sdk/utilities/circular_buffer.h b/roborts_base/roborts_sdk/utilities/circular_buffer.h new file mode 100644 index 00000000..e5f192fe --- /dev/null +++ b/roborts_base/roborts_sdk/utilities/circular_buffer.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_CIRCULAR_BUFFER_H +#define ROBORTS_SDK_CIRCULAR_BUFFER_H + +#include +#include +/** + * @brief CircularBuffer for certain command + * @tparam T Certain Data type used in the buffer + */ +template +class CircularBuffer { + public: + /** + * @brief Constructor of circular buffer + * @param size + */ + explicit CircularBuffer(size_t size) : + buf_(std::unique_ptr(new T[size])), + max_size_(size) + {} + /** + * @brief Push the data into the buffer + * @param item Item to be pushed into the buffer + */ + void Push(T item) + { + std::lock_guard lock(mutex_); + + buf_[head_] = item; + + if(full_) + { + tail_ = (tail_ + 1) % max_size_; + } + + head_ = (head_ + 1) % max_size_; + + full_ = head_ == tail_; + } + /** + * @brief Pop the data from the buffer and get the popped item + * @param item Item to be popped from the buffer + * @return True if buffer is not empty + */ + bool Pop(T &item) + { + std::lock_guard lock(mutex_); + + if(IsEmpty()) + { + return false; + } + + //Read data and advance the tail (we now have a free space) + item = buf_[tail_]; + full_ = false; + tail_ = (tail_ + 1) % max_size_; + + return true; + } + /** + * @brief Reset the buffer + */ + void Reset() + { + std::lock_guard lock(mutex_); + head_ = tail_; + full_ = false; + } + /** + * @brief Decide whether the buffer is empty + * @return True if empty + */ + bool IsEmpty() const + { + //if head and tail are equal, we are empty + return (!full_ && (head_ == tail_)); + } + /** + * @brief Decide whether the buffer is full + * @return True if full + */ + bool IsFull() const + { + //If tail is ahead the head by 1, we are full + return full_; + } + /** + * @brief Get the buffer capacity(max size of the buffer) + * @return The buffer capacity + */ + size_t GetCapacity() const + { + return max_size_; + } + /** + * @brief Get the current size of the buffer + * @return The current size of the buffer + */ + size_t GetSize() const + { + size_t size = max_size_; + + if(!full_) + { + if(head_ >= tail_) + { + size = head_ - tail_; + } + else + { + size = max_size_ + head_ - tail_; + } + } + + return size; + } + + private: + //! mutex of the buffer + std::mutex mutex_; + //! buffer pointer + std::unique_ptr buf_; + //! buffer head + size_t head_ = 0; + //! buffer tail + size_t tail_ = 0; + //! buffer capacity + size_t max_size_; + //! flag of full buffer + bool full_ = 0; +}; + +#endif //ROBORTS_SDK_CIRCULAR_BUFFER_H diff --git a/roborts_base/roborts_sdk/utilities/crc.h b/roborts_base/roborts_sdk/utilities/crc.h new file mode 100644 index 00000000..00fba441 --- /dev/null +++ b/roborts_base/roborts_sdk/utilities/crc.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_CRC_H +#define ROBORTS_SDK_CRC_H +//---------------------------------------------------------------------- +// CRC Management +//---------------------------------------------------------------------- + +//! CRC16_IBM standard, polygon code 0x8005 +const uint16_t crc_tab16[] = { + 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, 0xc601, + 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, 0xcc01, 0x0cc0, + 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, 0x0a00, 0xcac1, 0xcb81, + 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, 0xd801, 0x18c0, 0x1980, 0xd941, + 0x1b00, 0xdbc1, 0xda81, 0x1a40, 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, + 0x1dc0, 0x1c80, 0xdc41, 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, + 0x1680, 0xd641, 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, + 0x1040, 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, + 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, 0x3c00, + 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, 0xfa01, 0x3ac0, + 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 0x2800, 0xe8c1, 0xe981, + 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 0xee01, 0x2ec0, 0x2f80, 0xef41, + 0x2d00, 0xedc1, 0xec81, 0x2c40, 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, + 0xe7c1, 0xe681, 0x2640, 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, + 0x2080, 0xe041, 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, + 0x6240, 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, + 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 0xaa01, + 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 0x7800, 0xb8c1, + 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, 0xbe01, 0x7ec0, 0x7f80, + 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, 0xb401, 0x74c0, 0x7580, 0xb541, + 0x7700, 0xb7c1, 0xb681, 0x7640, 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, + 0x71c0, 0x7080, 0xb041, 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, + 0x5280, 0x9241, 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, + 0x5440, 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, + 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, 0x8801, + 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, 0x4e00, 0x8ec1, + 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 0x4400, 0x84c1, 0x8581, + 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 0x8201, 0x42c0, 0x4380, 0x8341, + 0x4100, 0x81c1, 0x8081, 0x4040 +}; + +//! CRC32_Common standard, polygon code 0x04C11DB7 +const uint32_t crc_tab32[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, + 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, + 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2, + 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, + 0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, + 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c, + 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, + 0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, + 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106, + 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, + 0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, + 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950, + 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, + 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, + 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa, + 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, + 0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, + 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84, + 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, + 0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, + 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e, + 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, + 0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, + 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28, + 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, + 0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, + 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242, + 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, + 0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, + 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc, + 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, + 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, + 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + +//! CRC8_Dallas_Maxim standard, note for changing this to a better implemtation +const uint8_t crc_tab8[256] = { + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, + 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, + 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, + 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 0xbe, 0xe0, 0x02, 0x5c, + 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 0x46, + 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, + 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, + 0xfb, 0x78, 0x26, 0xc4, 0x9a, 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, + 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, + 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, + 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, + 0xcd, 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, + 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, + 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, + 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, 0xca, 0x94, 0x76, + 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, + 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, + 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, + 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, + 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, +}; + + +const uint8_t CRC8_INIT = 0x77; +const uint16_t CRC16_INIT = 0x3692; +const uint16_t CRC_INIT = 0x3AA3; + +#endif //ROBORTS_SDK_CRC_H diff --git a/roborts_base/roborts_sdk/utilities/log.h b/roborts_base/roborts_sdk/utilities/log.h new file mode 100644 index 00000000..da89669e --- /dev/null +++ b/roborts_base/roborts_sdk/utilities/log.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_LOG_H +#define ROBORTS_SDK_LOG_H +#include +#include +#include "glog/logging.h" +#include "glog/raw_logging.h" + + +#define LOG_INFO LOG(INFO) +#define LOG_WARNING LOG(WARNING) +#define LOG_ERROR LOG(ERROR) +#define LOG_FATAL LOG(FATAL) + +#define LOG_INFO_IF(condition) LOG_IF(INFO,condition) +#define LOG_WARNING_IF(condition) LOG_IF(WARNING,condition) +#define LOG_ERROR_IF(condition) LOG_IF(ERROR,condition) +#define LOG_FATAL_IF(condition) LOG_IF(FATAL,condition) + +#define LOG_INFO_EVERY(freq) LOG_EVERY_N(INFO, freq) +#define LOG_WARNING_EVERY(freq) LOG_EVERY_N(WARNING, freq) +#define LOG_ERROR_EVERY(freq) LOG_EVERY_N(ERROR, freq) + +#define DLOG_INFO DLOG(INFO) +#define DLOG_WARNING DLOG(WARNING) +#define DLOG_ERROR DLOG(WARNING) + +#define LOG_WARNING_FIRST_N(times) LOG_FIRST_N(WARNING, times) + + +class GLogWrapper { + public: + GLogWrapper(char* program) { + google::InitGoogleLogging(program); + FLAGS_stderrthreshold=google::WARNING; + FLAGS_colorlogtostderr=true; + FLAGS_v = 3; + google::InstallFailureSignalHandler(); + } + + ~GLogWrapper() + { + google::ShutdownGoogleLogging(); + } +}; + + +#endif //ROBORTS_SDK_LOG_H diff --git a/roborts_base/roborts_sdk/utilities/memory_pool.h b/roborts_base/roborts_sdk/utilities/memory_pool.h new file mode 100644 index 00000000..dad5e98f --- /dev/null +++ b/roborts_base/roborts_sdk/utilities/memory_pool.h @@ -0,0 +1,271 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_MEMORY_POOL_H +#define ROBORTS_SDK_MEMORY_POOL_H +#include +#include +#include +/** + * @brief Memory block information + */ +typedef struct MemoryBlock +{ + //! flag of usage + bool usage_flag; + //! memory block index in the table + uint8_t table_index; + //! memory size + uint16_t memory_size; + //! memory pointer + uint8_t* memory_ptr; +} MemoryBlock; + +/** + * @brief Memory pool to manage memory blocks + */ +class MemoryPool { + public: + /** + * @brief Constructor of memory pool + * @param max_block_size Max size for single memory block + * @param memory_size Max memory size + * @param memory_table_num Memory block number in the table + */ + MemoryPool(uint16_t max_block_size = 1024, + uint16_t memory_size = 1024, + uint16_t memory_table_num = 32): + memory_size_(memory_size), + memory_table_num_(memory_table_num), + max_block_size_(max_block_size) + { + }; + /** + * @brief Destructor of memory pool + */ + ~MemoryPool(){ + delete []memory_; + delete []memory_table_; + } + /** + * @brief Initialization of memory table + */ + void Init() + { + memory_table_ = new MemoryBlock[memory_table_num_]; + memory_ = new uint8_t[memory_size_]; + uint32_t i; + memory_table_[0].table_index = 0; + memory_table_[0].usage_flag = 1; + memory_table_[0].memory_ptr = memory_; + memory_table_[0].memory_size = 0; + for (i = 1; i < (memory_table_num_ - 1); i++) + { + memory_table_[i].table_index = i; + memory_table_[i].usage_flag = 0; + } + memory_table_[memory_table_num_ - 1].table_index = memory_table_num_ - 1; + memory_table_[memory_table_num_ - 1].usage_flag = 1; + memory_table_[memory_table_num_ - 1].memory_ptr = memory_ + memory_size_; + memory_table_[memory_table_num_ - 1].memory_size = 0; + } + /** + * @brief Free certain memory block in the pool + * @param memory_block Memory block to be freed + */ + void FreeMemory(MemoryBlock* memory_block) + { + if (memory_block == (MemoryBlock*)0) + { + return; + } + if (memory_block->table_index == 0 || + memory_block->table_index == (memory_table_num_ - 1)) + { + return; + } + memory_block->usage_flag = 0; + } + /** + * @brief Allocate a memory block in the pool + * @param size size of the memory block + * @return pointer of memory block + */ + MemoryBlock* AllocMemory(uint16_t size) + { + uint32_t used_memory_size = 0; + uint8_t i = 0; + uint8_t j = 0; + uint8_t memory_table_used_num = 0; + uint8_t memory_table_used_index[memory_table_num_]; + + uint32_t block_memory_size; + uint32_t temp_area[2] = { 0xFFFFFFFF, 0xFFFFFFFF }; + uint32_t accumulate_left_memory_size = 0; + bool index_found = false; + + // If size is larger than memory size or max size for PACKAGE, allocate failed + if (size>max_block_size_||size > memory_size_) + { + return (MemoryBlock *) 0; + } + + // Calculate the used memory size and get the used index array + for (i = 0; i < memory_table_num_; i++) + { + if (memory_table_[i].usage_flag == 1) + { + used_memory_size += memory_table_[i].memory_size; + memory_table_used_index[memory_table_used_num++] = memory_table_[i].table_index; + } + } + + // If left size is smaller than needed, allocate failed + if (memory_size_ < (used_memory_size + size)) + { + return (MemoryBlock *) 0; + } + + // Special case: allocate for the first time + if (used_memory_size == 0) + { + memory_table_[1].memory_ptr = memory_table_[0].memory_ptr; + memory_table_[1].memory_size = size; + memory_table_[1].usage_flag = 1; + return &memory_table_[1]; + } + + //memory_table is out of order, memory_table_used_index is ordered by its memory allocation order + for (i = 0; i < (memory_table_used_num - 1); i++) + { + for (j = 0; j < (memory_table_used_num - i - 1); j++) + { + if (memory_table_[memory_table_used_index[j]].memory_ptr > + memory_table_[memory_table_used_index[j + 1]].memory_ptr) + { + memory_table_used_index[j + 1] ^= memory_table_used_index[j]; + memory_table_used_index[j] ^= memory_table_used_index[j + 1]; + memory_table_used_index[j + 1] ^= memory_table_used_index[j]; + } + } + } + + for (i = 0; i < (memory_table_used_num - 1); i++) + { + //Find memory size for each block + block_memory_size = static_cast(memory_table_[memory_table_used_index[i + 1]].memory_ptr - + memory_table_[memory_table_used_index[i]].memory_ptr); + + //Check if block left memory size is enough for needed size, if so, then record block table id and its left memory + if ((block_memory_size - memory_table_[memory_table_used_index[i]].memory_size) >= size) + { + if (temp_area[1] > (block_memory_size - memory_table_[memory_table_used_index[i]].memory_size)) + { + temp_area[0] = memory_table_[memory_table_used_index[i]].table_index; + temp_area[1] = block_memory_size - memory_table_[memory_table_used_index[i]].memory_size; + } + } + + //accumulate the left memory size for each block + accumulate_left_memory_size += block_memory_size - memory_table_[memory_table_used_index[i]].memory_size; + //record the index when accumulate_left_memory_size is larger than needed size for the first time + if (accumulate_left_memory_size >= size && !index_found) + { + j = i; + index_found = true; + } + } + + //If no single block is available to divide for needed size, then compress the table according to accumulate memory + if (temp_area[0] == 0xFFFFFFFF && temp_area[1] == 0xFFFFFFFF) + { + for (i = 0; i < j; i++) + { + if (memory_table_[memory_table_used_index[i + 1]].memory_ptr > + (memory_table_[memory_table_used_index[i]].memory_ptr + + memory_table_[memory_table_used_index[i]].memory_size)) + { + memmove(memory_table_[memory_table_used_index[i]].memory_ptr + + memory_table_[memory_table_used_index[i]].memory_size, + memory_table_[memory_table_used_index[i + 1]].memory_ptr, + memory_table_[memory_table_used_index[i + 1]].memory_size); + memory_table_[memory_table_used_index[i + 1]].memory_ptr = + memory_table_[memory_table_used_index[i]].memory_ptr + + memory_table_[memory_table_used_index[i]].memory_size; + } + } + + for (i = 1; i < (memory_table_num_ - 1); i++) + { + if (memory_table_[i].usage_flag == 0) + { + memory_table_[i].memory_ptr = memory_table_[memory_table_used_index[j]].memory_ptr + + memory_table_[memory_table_used_index[j]].memory_size; + + memory_table_[i].memory_size = size; + memory_table_[i].usage_flag = 1; + return &memory_table_[i]; + } + } + return (MemoryBlock*)0; + } + + //If single block is available to divide for needed size, then divide this block into two + for (i = 1; i < (memory_table_num_ - 1); i++) + { + if (memory_table_[i].usage_flag == 0) + { + memory_table_[i].memory_ptr = + memory_table_[temp_area[0]].memory_ptr + memory_table_[temp_area[0]].memory_size; + + memory_table_[i].memory_size = size; + memory_table_[i].usage_flag = 1; + return &memory_table_[i]; + } + } + + return (MemoryBlock*)0; + } + /** + * @brief Lock the memory pool + */ + void LockMemory(){ + memory_mutex_.lock(); + } +/** + * @brief Unlock the memor pool + */ + void UnlockMemory(){ + memory_mutex_.unlock(); + } + + private: + //! mutex of the memory pool + std::mutex memory_mutex_; + //! number of memory block in the table + uint16_t memory_table_num_; + //! max memory size + uint16_t memory_size_; + //! max size for single memory block + uint16_t max_block_size_; + //! memory table pointer + MemoryBlock* memory_table_; + //! memory pointer + uint8_t* memory_; +}; + +#endif //ROBORTS_SDK_MEMORY_POOL_H diff --git a/roborts_base/ros_dep.h b/roborts_base/ros_dep.h new file mode 100644 index 00000000..cb020c08 --- /dev/null +++ b/roborts_base/ros_dep.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H +#define ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H +#include +#include + +#include +#include +#include + +#include "roborts_msgs/GimbalAngle.h" +#include "roborts_msgs/GimbalRate.h" + +#include "roborts_msgs/ChassisMode.h" +#include "roborts_msgs/TwistAccel.h" +#include "roborts_msgs/GimbalMode.h" +#include "roborts_msgs/ShootCmd.h" +#include "roborts_msgs/FricWhl.h" + +#endif //ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H diff --git a/roborts_bringup/CMakeLists.txt b/roborts_bringup/CMakeLists.txt new file mode 100755 index 00000000..f24e6f7a --- /dev/null +++ b/roborts_bringup/CMakeLists.txt @@ -0,0 +1,19 @@ +project(roborts_bringup) +cmake_minimum_required(VERSION 3.1) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) + +find_package(catkin REQUIRED COMPONENTS + roborts_base + roborts_common + roborts_localization + roborts_costmap + roborts_msgs + roborts_planning + ) + +catkin_package() + + diff --git a/roborts_bringup/launch/base.launch b/roborts_bringup/launch/base.launch new file mode 100755 index 00000000..ed7907ca --- /dev/null +++ b/roborts_bringup/launch/base.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/roborts_bringup/launch/mapping.launch b/roborts_bringup/launch/mapping.launch new file mode 100755 index 00000000..12184461 --- /dev/null +++ b/roborts_bringup/launch/mapping.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/roborts_bringup/launch/mapping_stage.launch b/roborts_bringup/launch/mapping_stage.launch new file mode 100755 index 00000000..4704cbf2 --- /dev/null +++ b/roborts_bringup/launch/mapping_stage.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/roborts_bringup/launch/roborts.launch b/roborts_bringup/launch/roborts.launch new file mode 100755 index 00000000..81d8c3d4 --- /dev/null +++ b/roborts_bringup/launch/roborts.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roborts_bringup/launch/roborts_stage.launch b/roborts_bringup/launch/roborts_stage.launch new file mode 100755 index 00000000..1241f3c2 --- /dev/null +++ b/roborts_bringup/launch/roborts_stage.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roborts_bringup/launch/slam_gmapping.xml b/roborts_bringup/launch/slam_gmapping.xml new file mode 100755 index 00000000..e437ef99 --- /dev/null +++ b/roborts_bringup/launch/slam_gmapping.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roborts_bringup/launch/static_tf.launch b/roborts_bringup/launch/static_tf.launch new file mode 100644 index 00000000..4cb3a99e --- /dev/null +++ b/roborts_bringup/launch/static_tf.launch @@ -0,0 +1,10 @@ + + + + + + + + \ No newline at end of file diff --git a/roborts_bringup/maps/icra2018.pgm b/roborts_bringup/maps/icra2018.pgm new file mode 100644 index 00000000..b747e7b5 Binary files /dev/null and b/roborts_bringup/maps/icra2018.pgm differ diff --git a/roborts_bringup/maps/icra2018.yaml b/roborts_bringup/maps/icra2018.yaml new file mode 100755 index 00000000..9cae9ee5 --- /dev/null +++ b/roborts_bringup/maps/icra2018.yaml @@ -0,0 +1,7 @@ +image: icra2018.pgm +resolution: 0.05 +origin: [0, 0, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/roborts_bringup/maps/icra2019.pgm b/roborts_bringup/maps/icra2019.pgm new file mode 100644 index 00000000..affdc81e Binary files /dev/null and b/roborts_bringup/maps/icra2019.pgm differ diff --git a/roborts_bringup/maps/icra2019.yaml b/roborts_bringup/maps/icra2019.yaml new file mode 100755 index 00000000..09152b07 --- /dev/null +++ b/roborts_bringup/maps/icra2019.yaml @@ -0,0 +1,7 @@ +image: icra2019.pgm +resolution: 0.05 +origin: [0, 0, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/roborts_bringup/package.xml b/roborts_bringup/package.xml new file mode 100755 index 00000000..5ee97d96 --- /dev/null +++ b/roborts_bringup/package.xml @@ -0,0 +1,32 @@ + + roborts_bringup + 1.0.0 + + The roborts_bringup package provides quick bringup for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + roborts_base + roborts_common + roborts_localization + roborts_costmap + roborts_msgs + roborts_planning + roborts_tf + + roborts_base + roborts_common + roborts_localization + roborts_costmap + roborts_msgs + roborts_tf + + + + + diff --git a/roborts_bringup/rviz/mapping.rviz b/roborts_bringup/rviz/mapping.rviz new file mode 100755 index 00000000..4355760a --- /dev/null +++ b/roborts_bringup/rviz/mapping.rviz @@ -0,0 +1,219 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 844 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_laser_link: + Value: 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_footprint: + base_link: + base_laser_link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /base_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.100000001 + 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.300000012 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.100000001 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Color: 255; 25; 0 + Head Length: 0.300000012 + Head Radius: 0.100000001 + Shaft Length: 1 + Shaft Radius: 0.0500000007 + Value: Axes + Topic: /odom + Unreliable: false + 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 + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 18.5378971 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.81789017 + Y: -2.11132455 + Z: -4.76837158e-06 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.56979632 + Target Frame: odom + Value: ThirdPersonFollower (rviz) + Yaw: 4.72214699 + Saved: + - Class: rviz/ThirdPersonFollower + Distance: 37.8721542 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.51163912 + Y: -6.79189587 + Z: -4.76837158e-06 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.00999999978 + Pitch: 1.56979632 + Target Frame: odom + Value: ThirdPersonFollower (rviz) + Yaw: 4.71213627 +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000003dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003da000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003da000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 24 diff --git a/roborts_bringup/rviz/roborts.rviz b/roborts_bringup/rviz/roborts.rviz new file mode 100755 index 00000000..721f98b6 --- /dev/null +++ b/roborts_bringup/rviz/roborts.rviz @@ -0,0 +1,225 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 714 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan + - Class: rviz/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_laser_link: + Value: 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_footprint: + base_link: + base_laser_link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: /local_costmap/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.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /global_planner_node/path + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /local_planner_node/local_planner + Unreliable: false + 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 + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 10.0383463 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.46299744 + Y: 3.80465364 + Z: 9.53674316e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.00100000005 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.229796529 + Target Frame: map + Value: ThirdPersonFollower (rviz) + Yaw: 2.02761245 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 926 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000358000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000353000000af0000006100ffffff000000010000010f00000272fc0200000002fb0000000a00560069006500770073000000002800000272000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005d20000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1858 + X: 20 + Y: 505 diff --git a/roborts_bringup/rviz/teb_test.rviz b/roborts_bringup/rviz/teb_test.rviz new file mode 100644 index 00000000..93083a87 --- /dev/null +++ b/roborts_bringup/rviz/teb_test.rviz @@ -0,0 +1,187 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Path1 + Splitter Ratio: 0.5 + Tree Height: 786 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + 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 + - Alpha: 0.100000001 + Cell Size: 0.100000001 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Fine Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.0599999987 + Head Length: 0.0500000007 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 170; 0; 0 + Pose Style: Arrows + Radius: 0.0299999993 + Shaft Diameter: 0.0199999996 + Shaft Length: 0.0799999982 + Topic: /trajectory + Unreliable: false + Value: true + - Alpha: 1 + Arrow Length: 0.300000012 + Axes Length: 0.300000012 + Axes Radius: 0.00999999978 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.0700000003 + Head Radius: 0.0299999993 + Name: PoseArray + Shaft Length: 0.230000004 + Shaft Radius: 0.00999999978 + Shape: Arrow (Flat) + Topic: /pose + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /test_optim_node/teb_markers + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /marker_obstacles/update + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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 + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 12.2185116 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.158661455 + Y: -0.0405401513 + Z: 3.99946366e-05 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.54479635 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.53543007 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a000003a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001a000003a3000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000026e00fffffffb0000000800540069006d0065010000000000000450000000000000000000000610000003a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 1920 + Y: 24 diff --git a/roborts_bringup/scripts/udev/create_udev_rules.sh b/roborts_bringup/scripts/udev/create_udev_rules.sh new file mode 100644 index 00000000..f39d4e80 --- /dev/null +++ b/roborts_bringup/scripts/udev/create_udev_rules.sh @@ -0,0 +1,14 @@ +#!/bin/bash +echo "Remap the device port(ttyUSBX) to alias" +echo "USB connection for serial" + +echo "Check them using the command : ls -l /dev|grep ttyUSB" +echo "Start to copy udev rules to /etc/udev/rules.d/" + +sudo cp `rospack find roborts_bringup`/scripts/udev/roborts.rules /etc/udev/rules.d +echo " " +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +echo "Finish " diff --git a/roborts_bringup/scripts/udev/delete_udev_rules.sh b/roborts_bringup/scripts/udev/delete_udev_rules.sh new file mode 100644 index 00000000..d60054bf --- /dev/null +++ b/roborts_bringup/scripts/udev/delete_udev_rules.sh @@ -0,0 +1,12 @@ +#!/bin/bash +echo "Delete remap the device port(ttyUSBX) to alias" +echo "USB connection for serial" + +echo "Delete udev in the /etc/udev/rules.d/" +sudo rm /etc/udev/rules.d/roborts.rules +echo " " +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +echo "Finish" diff --git a/roborts_bringup/scripts/udev/roborts.rules b/roborts_bringup/scripts/udev/roborts.rules new file mode 100644 index 00000000..104c5db1 --- /dev/null +++ b/roborts_bringup/scripts/udev/roborts.rules @@ -0,0 +1,3 @@ +# set the udev rule +KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0777", SYMLINK+="serial_sdk" +KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar" diff --git a/roborts_bringup/scripts/upstart/create_upstart_service.sh b/roborts_bringup/scripts/upstart/create_upstart_service.sh new file mode 100644 index 00000000..ad7090b0 --- /dev/null +++ b/roborts_bringup/scripts/upstart/create_upstart_service.sh @@ -0,0 +1,17 @@ +#!/bin/bash +echo " " +echo "Start to copy script files to /home/dji" +echo "" +cp `rospack find roborts_bringup`/scripts/upstart/roborts-start.sh /home/dji +cp `rospack find roborts_bringup`/scripts/upstart/max_performance.sh /home/dji +echo " " +echo "Start to copy service files to /lib/systemd/system/" +echo "" +sudo cp `rospack find roborts_bringup`/scripts/upstart/max-performance.service /lib/systemd/system/ +sudo cp `rospack find roborts_bringup`/scripts/upstart/roborts.service /lib/systemd/system/ +echo " " +echo "Enable max-performance and roborts service for upstart! " +echo "" +sudo systemctl enable max-performance.service +sudo systemctl enable roborts.service +echo "Finish " diff --git a/roborts_bringup/scripts/upstart/delete_upstart_service.sh b/roborts_bringup/scripts/upstart/delete_upstart_service.sh new file mode 100644 index 00000000..bcb374d0 --- /dev/null +++ b/roborts_bringup/scripts/upstart/delete_upstart_service.sh @@ -0,0 +1,17 @@ +#!/bin/bash +echo " " +echo "Start to remove script files from /home/dji" +echo "" +rm /home/dji/roborts-start.sh +rm /home/dji/max_performance.sh +echo " " +echo "Start to remove service files from /lib/systemd/system/" +echo "" +sudo rm /lib/systemd/system/max-performance.service +sudo rm /lib/systemd/system/roborts.service +echo " " +echo "Disable max-performance and roborts service for upstart! " +echo "" +sudo systemctl disable max-performance.service +sudo systemctl disable roborts.service +echo "Finish" diff --git a/roborts_bringup/scripts/upstart/jetson_clocks.sh b/roborts_bringup/scripts/upstart/jetson_clocks.sh new file mode 100755 index 00000000..7c1c79a1 --- /dev/null +++ b/roborts_bringup/scripts/upstart/jetson_clocks.sh @@ -0,0 +1,401 @@ +#!/bin/bash +# Copyright (c) 2015-2017, NVIDIA CORPORATION. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of NVIDIA CORPORATION nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY +# EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +# PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY +# OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +CONF_FILE=${HOME}/l4t_dfs.conf +RED='\e[0;31m' +GREEN='\e[0;32m' +BLUE='\e[0;34m' +BRED='\e[1;31m' +BGREEN='\e[1;32m' +BBLUE='\e[1;34m' +NC='\e[0m' # No Color + +usage() +{ + if [ "$1" != "" ]; then + echo -e ${RED}"$1"${NC} + fi + + cat >& 2 < "${file}" + fi + ;; + /sys/devices/system/cpu/cpu*/cpufreq/scaling_min_freq |\ + /sys/kernel/debug/tegra_cpufreq/*_CLUSTER/cc3/enable) + echo "${data}" > "${file}" 2>/dev/null + ;; + *) + echo "${data}" > "${file}" + ret=$? + if [ ${ret} -ne 0 ]; then + echo "Error: Failed to restore $file" + fi + ;; + esac + done +} + +store() +{ + for file in $@; do + if [ -e "${file}" ]; then + echo "${file}:`cat ${file}`" >> "${CONF_FILE}" + fi + done +} + +do_fan() +{ + # Jetson-TK1 CPU fan is always ON. + if [ "${machine}" = "jetson-tk1" ] ; then + return + fi + + if [ ! -w /sys/kernel/debug/tegra_fan/target_pwm ]; then + echo "Can't access Fan!" + return + fi + + case "${ACTION}" in + show) + echo "Fan: speed=`cat /sys/kernel/debug/tegra_fan/target_pwm`" + ;; + store) + store "/sys/kernel/debug/tegra_fan/target_pwm" + ;; + *) + FAN_SPEED=255 + echo "${FAN_SPEED}" > /sys/kernel/debug/tegra_fan/target_pwm + ;; + esac +} + +do_clusterswitch() +{ + case "${ACTION}" in + show) + if [ -d "/sys/kernel/cluster" ]; then + ACTIVE_CLUSTER=`cat /sys/kernel/cluster/active` + echo "CPU Cluster Switching: Active Cluster ${ACTIVE_CLUSTER}" + else + echo "CPU Cluster Switching: Disabled" + fi + ;; + store) + if [ -d "/sys/kernel/cluster" ]; then + store "/sys/kernel/cluster/immediate" + store "/sys/kernel/cluster/force" + store "/sys/kernel/cluster/active" + fi + ;; + *) + if [ -d "/sys/kernel/cluster" ]; then + echo 1 > /sys/kernel/cluster/immediate + echo 0 > /sys/kernel/cluster/force + echo G > /sys/kernel/cluster/active + fi + ;; + esac +} + +do_hotplug() +{ + case "${ACTION}" in + show) + echo "Online CPUs: `cat /sys/devices/system/cpu/online`" + ;; + store) + for file in /sys/devices/system/cpu/cpu[0-9]/online; do + store "${file}" + done + ;; + *) + if [ "${SOCFAMILY}" != "tegra186" ]; then + for file in /sys/devices/system/cpu/cpu*/online; do + if [ `cat $file` -eq 0 ]; then + echo 1 > "${file}" + fi + done + fi + esac +} + +do_cpu() +{ + FREQ_GOVERNOR="cpufreq/scaling_governor" + CPU_MIN_FREQ="cpufreq/scaling_min_freq" + CPU_MAX_FREQ="cpufreq/scaling_max_freq" + CPU_CUR_FREQ="cpufreq/scaling_cur_freq" + CPU_SET_SPEED="cpufreq/scaling_setspeed" + INTERACTIVE_SETTINGS="/sys/devices/system/cpu/cpufreq/interactive" + SCHEDUTIL_SETTINGS="/sys/devices/system/cpu/cpufreq/schedutil" + + case "${ACTION}" in + show) + for folder in /sys/devices/system/cpu/cpu[0-9]; do + CPU=`basename ${folder}` + if [ -e "${folder}/${FREQ_GOVERNOR}" ]; then + echo "$CPU: Gonvernor=`cat ${folder}/${FREQ_GOVERNOR}`" \ + "MinFreq=`cat ${folder}/${CPU_MIN_FREQ}`" \ + "MaxFreq=`cat ${folder}/${CPU_MAX_FREQ}`" \ + "CurrentFreq=`cat ${folder}/${CPU_CUR_FREQ}`" + fi + done + ;; + store) + store "/sys/module/qos/parameters/enable" + + for file in \ + /sys/devices/system/cpu/cpu[0-9]/cpufreq/scaling_min_freq; do + store "${file}" + done + + if [ "${SOCFAMILY}" = "tegra186" ]; then + store "/sys/kernel/debug/tegra_cpufreq/M_CLUSTER/cc3/enable" + store "/sys/kernel/debug/tegra_cpufreq/B_CLUSTER/cc3/enable" + fi + ;; + *) + echo 0 > /sys/module/qos/parameters/enable + + if [ "${SOCFAMILY}" = "tegra186" ]; then + echo 0 > /sys/kernel/debug/tegra_cpufreq/M_CLUSTER/cc3/enable 2>/dev/null + echo 0 > /sys/kernel/debug/tegra_cpufreq/B_CLUSTER/cc3/enable 2>/dev/null + fi + + for folder in /sys/devices/system/cpu/cpu[0-9]; do + cat "${folder}/${CPU_MAX_FREQ}" > "${folder}/${CPU_MIN_FREQ}" 2>/dev/null + done + ;; + esac +} + +do_gpu() +{ + case "${SOCFAMILY}" in + tegra186) + GPU_MIN_FREQ="/sys/devices/17000000.gp10b/devfreq/17000000.gp10b/min_freq" + GPU_MAX_FREQ="/sys/devices/17000000.gp10b/devfreq/17000000.gp10b/max_freq" + GPU_CUR_FREQ="/sys/devices/17000000.gp10b/devfreq/17000000.gp10b/cur_freq" + GPU_RAIL_GATE="/sys/devices/17000000.gp10b/railgate_enable" + ;; + tegra210) + GPU_MIN_FREQ="/sys/devices/57000000.gpu/devfreq/57000000.gpu/min_freq" + GPU_MAX_FREQ="/sys/devices/57000000.gpu/devfreq/57000000.gpu/max_freq" + GPU_CUR_FREQ="/sys/devices/57000000.gpu/devfreq/57000000.gpu/cur_freq" + GPU_RAIL_GATE="/sys/devices/57000000.gpu/railgate_enable" + ;; + *) + echo "Error! unsupported SOC ${SOCFAMILY}" + exit 1; + ;; + esac + + case "${ACTION}" in + show) + echo "GPU MinFreq=`cat ${GPU_MIN_FREQ}`" \ + "MaxFreq=`cat ${GPU_MAX_FREQ}`" \ + "CurrentFreq=`cat ${GPU_CUR_FREQ}`" + ;; + store) + store "${GPU_MIN_FREQ}" + store "${GPU_RAIL_GATE}" + ;; + *) + echo 0 > "${GPU_RAIL_GATE}" + cat "${GPU_MAX_FREQ}" > "${GPU_MIN_FREQ}" + ret=$? + if [ ${ret} -ne 0 ]; then + echo "Error: Failed to max GPU frequency!" + fi + ;; + esac +} + +do_emc() +{ + case "${SOCFAMILY}" in + tegra186) + EMC_ISO_CAP="/sys/kernel/nvpmodel_emc_cap/emc_iso_cap" + EMC_MIN_FREQ="/sys/kernel/debug/bpmp/debug/clk/emc/min_rate" + EMC_MAX_FREQ="/sys/kernel/debug/bpmp/debug/clk/emc/max_rate" + EMC_CUR_FREQ="/sys/kernel/debug/clk/emc/clk_rate" + EMC_UPDATE_FREQ="/sys/kernel/debug/bpmp/debug/clk/emc/rate" + EMC_FREQ_OVERRIDE="/sys/kernel/debug/bpmp/debug/clk/emc/mrq_rate_locked" + ;; + tegra210) + EMC_MIN_FREQ="/sys/kernel/debug/tegra_bwmgr/emc_min_rate" + EMC_MAX_FREQ="/sys/kernel/debug/tegra_bwmgr/emc_max_rate" + EMC_CUR_FREQ="/sys/kernel/debug/clk/override.emc/clk_rate" + EMC_UPDATE_FREQ="/sys/kernel/debug/clk/override.emc/clk_update_rate" + EMC_FREQ_OVERRIDE="/sys/kernel/debug/clk/override.emc/clk_state" + ;; + *) + echo "Error! unsupported SOC ${SOCFAMILY}" + exit 1; + ;; + + esac + case "${ACTION}" in + show) + echo "EMC MinFreq=`cat ${EMC_MIN_FREQ}`" \ + "MaxFreq=`cat ${EMC_MAX_FREQ}`" \ + "CurrentFreq=`cat ${EMC_CUR_FREQ}`" \ + "FreqOverride=`cat ${EMC_FREQ_OVERRIDE}`" + ;; + store) + store "${EMC_FREQ_OVERRIDE}" + ;; + *) + if [ "${SOCFAMILY}" = "tegra186" ]; then + emc_cap=`cat "${EMC_ISO_CAP}"` + if [ "$emc_cap" -eq 0 ]; then + cat "${EMC_MAX_FREQ}" > "${EMC_UPDATE_FREQ}" + else + echo "$emc_cap" > "${EMC_UPDATE_FREQ}" + fi + else + cat "${EMC_MAX_FREQ}" > "${EMC_UPDATE_FREQ}" + fi + + echo 1 > "${EMC_FREQ_OVERRIDE}" + ;; + esac +} + +main () +{ + while [ -n "$1" ]; do + case "$1" in + --show) + echo "SOC family:${SOCFAMILY} Machine:${machine}" + ACTION=show + ;; + --store) + [ -n "$2" ] && CONF_FILE=$2 + ACTION=store + shift 1 + ;; + --restore) + [ -n "$2" ] && CONF_FILE=$2 + ACTION=restore + shift 1 + ;; + -h|--help) + usage + exit 0 + ;; + *) + usage "Unknown option: $1" + exit 1 + ;; + esac + shift 1 + done + + [ `whoami` != root ] && \ + echo Error: Run this script\($0\) as a root user && exit 1 + + case $ACTION in + store) + if [ -e "${CONF_FILE}" ]; then + echo "File $CONF_FILE already exists. Can I overwrite it? Y/N:" + read answer + case $answer in + y|Y) + rm -f $CONF_FILE + ;; + *) + echo "Error: file $CONF_FILE already exists!" + exit 1 + ;; + esac + fi + ;; + restore) + if [ ! -e "${CONF_FILE}" ]; then + echo "Error: $CONF_FILE file not found !" + exit 1 + fi + restore + exit 0 + ;; + esac + + do_hotplug + do_clusterswitch + do_cpu + do_gpu + do_emc + do_fan +} + +if [ -e "/sys/devices/soc0/family" ]; then + SOCFAMILY="`cat /sys/devices/soc0/family`" + if [ -e "/sys/devices/soc0/machine" ]; then + machine=`cat /sys/devices/soc0/machine` + fi +elif [ -e "/proc/device-tree/compatible" ]; then + grep "nvidia,tegra210" /proc/device-tree/compatible &>/dev/null + if [ $? -eq 0 ]; then + SOCFAMILY="tegra210" + else + grep "nvidia,tegra186" /proc/device-tree/compatible &>/dev/null + if [ $? -eq 0 ]; then + SOCFAMILY="tegra186" + fi + fi + + if [ -e "/proc/device-tree/model" ]; then + machine="`cat /proc/device-tree/model`" + fi +fi + +main $@ +exit 0 diff --git a/roborts_bringup/scripts/upstart/max-performance.service b/roborts_bringup/scripts/upstart/max-performance.service new file mode 100644 index 00000000..b45bfa05 --- /dev/null +++ b/roborts_bringup/scripts/upstart/max-performance.service @@ -0,0 +1,9 @@ +[Unit] +Description=Turn up performance +After=multi-user.target +[Service] +Type=oneshot +ExecStart=/bin/sh /home/dji/max_performance.sh + +[Install] +WantedBy=multi-user.target diff --git a/roborts_bringup/scripts/upstart/max_performance.sh b/roborts_bringup/scripts/upstart/max_performance.sh new file mode 100755 index 00000000..ac6fbf07 --- /dev/null +++ b/roborts_bringup/scripts/upstart/max_performance.sh @@ -0,0 +1,8 @@ +#/usr/bash +sleep 10 +echo dji | sudo -S sudo "/home/dji/jetson_clocks.sh" +echo "Max performance" +# disable wifi power saving + +echo dji | sudo -S sudo iw dev wlan0 set power_save off +echo "Set power_save off " diff --git a/roborts_bringup/scripts/upstart/roborts-start.sh b/roborts_bringup/scripts/upstart/roborts-start.sh new file mode 100755 index 00000000..5fad9ea8 --- /dev/null +++ b/roborts_bringup/scripts/upstart/roborts-start.sh @@ -0,0 +1,2 @@ +#!/bin/bash +bash -c "source /home/dji/.bashrc && roslaunch roborts_bringup roborts.launch" diff --git a/roborts_bringup/scripts/upstart/roborts.service b/roborts_bringup/scripts/upstart/roborts.service new file mode 100755 index 00000000..133d4da7 --- /dev/null +++ b/roborts_bringup/scripts/upstart/roborts.service @@ -0,0 +1,11 @@ +[Unit] +Description=roborts startup +After=network-online.target +[Service] +User=root +Type=simple +Restart=on-failure +ExecStart=/home/dji/roborts-start.sh + +[Install] +WantedBy=multi-user.target diff --git a/roborts_bringup/worlds/icra2018.world b/roborts_bringup/worlds/icra2018.world new file mode 100755 index 00000000..fc760c79 --- /dev/null +++ b/roborts_bringup/worlds/icra2018.world @@ -0,0 +1,86 @@ +define block model +( + size [0.4 0.4 1.2] + ranger_return 1 +) + +define frontcamera camera +( + size [ 0.050 0.050 0.0500 ] + range [ 0.301 8.0 ] + resolution [ 640 480 ] + fov [ 120 40 ] + pantilt [ 0 0 ] + alwayson 1 +) + +define rplidar ranger +( + sensor( + range_max 8.0 + fov 360 + samples 360 + ) + # generic model properties + color "black" + size [ 0.050 0.050 0.100 ] +) + +define rmcar position +( + size [0.6 0.45 0.460] + origin [0 0 0 0] + gui_nose 1 + drive "omni" + # frontcamera(pose [ 0 0 0 0 ]) + rplidar(pose [ 0 0 0 0 ]) + odom_error [0.03 0.03 0.00 0.05] + # [ xmin xmax ymin ymax zmin zmax amin amax ] + velocity_bounds [-2 2 -2 2 -2 2 -90 90 ] + acceleration_bounds [-2 2 -2 2 -2 2 -90 90] + ranger_return 1 +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 0 + + gui_nose 0 + gui_grid 0 + + gui_outline 0 + gripper_return 0 + fiducial_return 0 + ranger_return 1 +) + +# set the resolution of the underlying raytrace model in meters +resolution 0.01 + +interval_sim 50#83 # simulation timestep in milliseconds +interval_real 50#83 + +window +( + size [ 745.000 448.000 ] + rotate [ 0 0 ] + scale 29 +) + +# load an environment bitmap +floorplan +( + name "RoboMaster Map" + bitmap "../maps/icra2018.pgm" + size [8.35 5.5 1.000] + pose [4.175 2.775 0 0 ] +) + +# throw in a robot + rmcar(pose [ 1 1 0 0 ] name "rmcar0" color "blue" ) +# throw in a block for test + block(pose [ 7.79 3.45 0 0.3 ] color "red" ) diff --git a/roborts_bringup/worlds/icra2019.world b/roborts_bringup/worlds/icra2019.world new file mode 100755 index 00000000..86dc37c0 --- /dev/null +++ b/roborts_bringup/worlds/icra2019.world @@ -0,0 +1,86 @@ +define block model +( + size [0.4 0.4 1.2] + ranger_return 1 +) + +define frontcamera camera +( + size [ 0.050 0.050 0.0500 ] + range [ 0.301 8.0 ] + resolution [ 640 480 ] + fov [ 120 40 ] + pantilt [ 0 0 ] + alwayson 1 +) + +define rplidar ranger +( + sensor( + range_max 8.0 + fov 270 + samples 270 + ) + # generic model properties + color "black" + size [ 0.050 0.050 0.100 ] +) + +define rmcar position +( + size [0.6 0.45 0.460] + origin [0 0 0 0] + gui_nose 1 + drive "omni" + # frontcamera(pose [ 0 0 0 0 ]) + rplidar(pose [ 0 0 0 0 ]) + odom_error [0.03 0.03 0.00 0.05] + # [ xmin xmax ymin ymax zmin zmax amin amax ] + velocity_bounds [-2 2 -2 2 -2 2 -90 90 ] + acceleration_bounds [-2 2 -2 2 -2 2 -90 90] + ranger_return 1 +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 0 + + gui_nose 0 + gui_grid 0 + + gui_outline 0 + gripper_return 0 + fiducial_return 0 + ranger_return 1 +) + +# set the resolution of the underlying raytrace model in meters +resolution 0.01 + +interval_sim 50#83 # simulation timestep in milliseconds +interval_real 50#83 + +window +( + size [ 745.000 448.000 ] + rotate [ 0 0 ] + scale 29 +) + +# load an environment bitmap +floorplan +( + name "RoboMaster Map" + bitmap "../maps/icra2019.pgm" + size [8.15 5.15 1.000] + pose [4.075 2.575 0 0 ] +) + +# throw in a robot + rmcar(pose [ 1 1 0 0 ] name "rmcar0" color "blue" ) +# throw in a block for test + block(pose [ 7.79 3.45 0 0.3 ] color "red" ) diff --git a/roborts_camera/CMakeLists.txt b/roborts_camera/CMakeLists.txt new file mode 100755 index 00000000..9b356aa5 --- /dev/null +++ b/roborts_camera/CMakeLists.txt @@ -0,0 +1,103 @@ +project(roborts_camera) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) +add_definitions(-w -Wno-dev) + +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + actionlib + roborts_common + cv_bridge + image_transport + ) + +find_package(ProtoBuf REQUIRED) +find_package(OpenCV 3 REQUIRED) + +catkin_package( + INCLUDE_DIRS +) + +add_subdirectory(uvc) + +#camera parameter +file(GLOB CameraParamProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/camera_param.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto + CameraParamProtoSrc + CameraParamProtoHds + ${CameraParamProtoFiles} + ) + +#camera param +add_library(roborts_camera_param + SHARED + ${CameraParamProtoSrc} + ${CameraParamProtoHds} + camera_param.cpp +) + +target_link_libraries(roborts_camera_param + PUBLIC + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ${PROTOBUF_LIBRARIES} +) + +target_include_directories(roborts_camera_param + PUBLIC + ${catkin_INCLUDE_DIRS} +) + +#camera_node +add_executable(${PROJECT_NAME}_node + camera_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + PRIVATE + driver::uvc_driver + roborts_camera_param + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +target_include_directories(${PROJECT_NAME}_node + PRIVATE + ${OpenCV_INCLUDE_DIRECTORIES} + ${catkin_INCLUDE_DIRS} +) + +#cap_img + +add_executable(image_capture_test + test/image_capture.cpp +) + +if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64" ) +target_link_libraries(image_capture_test + PRIVATE + driver::mercure +) +endif() + +target_link_libraries(image_capture_test + PRIVATE + driver::uvc_driver + roborts_camera_param + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +target_include_directories(image_capture_test + PRIVATE + ${OpenCV_INCLUDE_DIRECTORIES} +) + +#install(DIRECTORY include/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) diff --git a/roborts_camera/camera_base.h b/roborts_camera/camera_base.h new file mode 100755 index 00000000..093fef40 --- /dev/null +++ b/roborts_camera/camera_base.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_CAMERA_CAMERA_BASE_H +#define ROBORTS_CAMERA_CAMERA_BASE_H + +#include +#include + +#include "state/error_code.h" + +namespace roborts_camera +{ +/** + * @brief Camera base class for the camera factory + */ +class CameraBase { + public: + /** + * @brief Constructor of CameraBase + * @param camera_info Information and parameters of camera + */ + explicit CameraBase(CameraInfo camera_info):camera_info_(camera_info),camera_initialized_(false){}; + virtual ~CameraBase() = default; + /** + * @brief Start to read camera + * @param img Image data in form of cv::Mat to be read + */ + virtual void StartReadCamera(cv::Mat &img) = 0; + + protected: + //! flag for camera initialization + bool camera_initialized_; + //! information and parameters of camera + CameraInfo camera_info_; +}; +} //namespace roborts_camera + + +#endif //ROBORTS_CAMERA_CAMERA_BASE_H \ No newline at end of file diff --git a/roborts_camera/camera_node.cpp b/roborts_camera/camera_node.cpp new file mode 100755 index 00000000..c464af43 --- /dev/null +++ b/roborts_camera/camera_node.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include + +#include "camera_node.h" + +namespace roborts_camera{ +CameraNode::CameraNode() { + camera_num_ = camera_param_.GetCameraParam().size(); + img_pubs_.resize(camera_num_); + camera_threads_.resize(camera_num_); + camera_driver_.resize(camera_num_); + + for (unsigned int i = 0; i < camera_num_; i++) { + auto camera_info = camera_param_.GetCameraParam()[i]; + nhs_.push_back(ros::NodeHandle(camera_info.camera_name)); + image_transport::ImageTransport it(nhs_.at(i)); + img_pubs_[i] = it.advertiseCamera("image_raw", 1, true); + //create the selected camera driver + camera_driver_[i] = roborts_common::AlgorithmFactory::CreateAlgorithm(camera_info.camera_type,camera_info); + } + + StartThread(); +} + +void CameraNode::StartThread() { + running_ = true; + for (unsigned int i = 0; i < camera_num_; i++) { + camera_threads_[i] = std::thread(&CameraNode::Update, this, i); + } +} + +void CameraNode::Update(const unsigned int index) { + cv::Mat img; + bool camera_info_send = false; + while(running_) { + camera_driver_[index]->StartReadCamera(img); + if(!img.empty()) { + sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); + img_msg->header.frame_id = camera_param_.GetCameraParam()[index].camera_name; + img_msg->header.stamp = ros::Time::now(); + + camera_param_.GetCameraParam()[index].ros_camera_info->header.stamp = img_msg->header.stamp; + img_pubs_[index].publish(img_msg, camera_param_.GetCameraParam()[index].ros_camera_info); + } + } +} + +void CameraNode::StoptThread() { +//TODO: To be implemented +} + +CameraNode::~CameraNode() { + running_ = false; + for (auto &iter: camera_threads_) { + if (iter.joinable()) + iter.join(); + } +} +} //namespace roborts_camera + +void SignalHandler(int signal){ + if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ + ros::shutdown(); + } +} + +int main(int argc, char **argv){ + signal(SIGINT, SignalHandler); + signal(SIGTERM,SignalHandler); + ros::init(argc, argv, "roborts_camera_node", ros::init_options::NoSigintHandler); + roborts_camera::CameraNode camera_test; + ros::AsyncSpinner async_spinner(1); + async_spinner.start(); + ros::waitForShutdown(); + return 0; +} diff --git a/roborts_camera/camera_node.h b/roborts_camera/camera_node.h new file mode 100755 index 00000000..3cdcca4a --- /dev/null +++ b/roborts_camera/camera_node.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_CAMERA_CAMERA_NODE_H +#define ROBORTS_CAMERA_CAMERA_NODE_H + +#include +#include + +#include +#include +#include +#include + +#include "uvc/uvc_driver.h" + +#include "camera_param.h" +#include "camera_base.h" +#include "alg_factory/algorithm_factory.h" +#include "io/io.h" + +namespace roborts_camera{ + +class CameraNode{ + public: + /** + * @brief Construcor of camera node + * @details Read the camera parameter, create the camera drivers based on camera factory + * and start all camera threads for update images. + */ + explicit CameraNode(); + /** + * @brief Start all the threads for camera drivers to update and publish the image data + */ + void StartThread(); + /** + * @brief Invoke each camera driver to read the image data and publish using ROS image_transport with corresponding parameter + */ + void Update(const unsigned int camera_num_); + /** + * @brief Stop to read image. + */ + void StoptThread(); + ~CameraNode(); + private: + //! drivers of different cameras inherited from camera base + std::vector> camera_driver_; + //! camera parameters + CameraParam camera_param_; + //! number of cameras + unsigned long camera_num_; + //! flag of running + bool running_; + //! threads of different cameras + std::vector camera_threads_; + + //! ROS node handlers of different cameras + std::vector nhs_; + //! ROS image transport camera publisher to publish image data + std::vector img_pubs_; + +}; +} //namespace roborts_camera + +#endif //ROBORTS_CAMERA_CAMERA_NODE_H diff --git a/roborts_camera/camera_param.cpp b/roborts_camera/camera_param.cpp new file mode 100755 index 00000000..88a757c5 --- /dev/null +++ b/roborts_camera/camera_param.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "camera_param.h" +#include "proto/camera_param.pb.h" + +#include "io/io.h" + +namespace roborts_camera{ + +CameraParam::CameraParam() { + LoadCameraParam(); +} + +void CameraParam::LoadCameraParam() { + Cameras camera_info; + std::string file_name = ros::package::getPath("roborts_camera") + "/config/camera_param.prototxt"; + bool read_state = roborts_common::ReadProtoFromTextFile(file_name, &camera_info); + ROS_ASSERT_MSG(read_state, "Cannot open %s", file_name.c_str()); + + //camera number and ids + int camera_num = camera_info.camera().size(); + cameras_param_.resize(camera_num); + for(unsigned int index = 0; index < camera_num; index++) { + + //camera id + cameras_param_[index].camera_name = camera_info.camera(index).camera_name(); + cameras_param_[index].camera_type = camera_info.camera(index).camera_type(); + cameras_param_[index].camera_path = camera_info.camera(index).camera_path(); + + //camera resolution + cameras_param_[index].resolution_width = camera_info.camera(index).resolution().width(); + cameras_param_[index].resolution_height = camera_info.camera(index).resolution().height(); + cameras_param_[index].width_offset = camera_info.camera(index).resolution().width_offset(); + cameras_param_[index].height_offset = camera_info.camera(index).resolution().height_offset(); + + //fps + cameras_param_[index].fps = camera_info.camera(index).fps(); + + //exposure + cameras_param_[index].auto_exposure = camera_info.camera(index).auto_exposure(); + cameras_param_[index].exposure_value = camera_info.camera(index).exposure_value(); + cameras_param_[index].exposure_time = camera_info.camera(index).exposure_time(); + + //white_balance and gain + cameras_param_[index].auto_white_balance = camera_info.camera(index).auto_white_balance(); + cameras_param_[index].auto_gain = camera_info.camera(index).auto_gain(); + + //constrast + cameras_param_[index].contrast = camera_info.camera(index).contrast(); + + //camera matrix + int camera_m_size = camera_info.camera(index).camera_matrix().data().size(); + double camera_m[camera_m_size]; + std::copy(camera_info.camera(index).camera_matrix().data().begin(), + camera_info.camera(index).camera_matrix().data().end(), + camera_m); + cameras_param_[index].camera_matrix = cv::Mat(3, 3, CV_64F, camera_m).clone(); + + //camera distortion + int rows = camera_info.camera(index).camera_distortion().data_size(); + double camera_dis[rows]; + std::copy(camera_info.camera(index).camera_distortion().data().begin(), + camera_info.camera(index).camera_distortion().data().end(), + camera_dis); + cameras_param_[index].camera_distortion = cv::Mat(rows, 1, CV_64F, camera_dis).clone(); + + //ros camera info + cameras_param_[index].ros_camera_info = boost::make_shared(); + cameras_param_[index].ros_camera_info->header.frame_id = cameras_param_[index].camera_name; + cameras_param_[index].ros_camera_info->width = cameras_param_[index].resolution_width; + cameras_param_[index].ros_camera_info->height = cameras_param_[index].resolution_height; + cameras_param_[index].ros_camera_info->D = cameras_param_[index].camera_distortion; + cameras_param_[index].ros_camera_info->roi.x_offset = cameras_param_[index].width_offset; + cameras_param_[index].ros_camera_info->roi.y_offset = cameras_param_[index].height_offset; + std::copy(camera_m, camera_m + camera_m_size, cameras_param_[index].ros_camera_info->K.begin()); + + } +} +std::vector& CameraParam::GetCameraParam() { + return cameras_param_; +} + +} //namespace roborts_camera diff --git a/roborts_camera/camera_param.h b/roborts_camera/camera_param.h new file mode 100755 index 00000000..1e8dd6bc --- /dev/null +++ b/roborts_camera/camera_param.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_CAMERA_CAMERA_PARAM_H +#define ROBORTS_CAMERA_CAMERA_PARAM_H + +#include +#include +#include +#include + +namespace roborts_camera { +/** + * @brief Information and parameter of camera + */ +struct CameraInfo { + //! name of camera + std::string camera_name; + //! type of camera for camera factory product instantiation + std::string camera_type; + //! path of camera, i.e. /dev/video0 + std::string camera_path; + //! camera matrix + cv::Mat camera_matrix; + //! camera distortion matrix + cv::Mat camera_distortion; + + //! resolution width + unsigned int resolution_width; + //! resolution height + unsigned int resolution_height; + + //! width offset for image crop + unsigned int width_offset; + //! height offset for image crop + unsigned int height_offset; + + //! camera fps + unsigned int fps; + //! flag of auto exposure + bool auto_exposure; + //! exposure value + unsigned int exposure_value; + //! exposure time + unsigned int exposure_time; + //! auto white balance + bool auto_white_balance; + //! auto gain + bool auto_gain; + //! contrast + unsigned int contrast; + + //! camera information in form of ROS sensor_msgs + sensor_msgs::CameraInfoPtr ros_camera_info; + //! opencv video capture + cv::VideoCapture cap_handle; +}; + +/** + * @brief camera parameter class to load and get camera parameters + */ +class CameraParam { + public: + /** + * @brief Constructor of CameraParam, load the camera parameter + */ + CameraParam(); + /** + * @brief load all camera parameters for different cameras + */ + void LoadCameraParam(); + /** + * @brief Get the camera parameters for different cameras + * @param cameras_param Camera parameters to update for different cameras + */ + void GetCameraParam(std::vector &cameras_param); + /** + * @brief Get the camera parameters for different cameras + * @return Updated Camera parameters for different cameras + */ + std::vector& GetCameraParam(); + ~CameraParam() = default; + private: + //! camera parameters for different cameras + std::vector cameras_param_; +}; + +} //namespace roborts_camera + +#endif //ROBORTS_CAMERA_CAMERA_PARAM_H diff --git a/roborts_camera/cmake_module/FindProtoBuf.cmake b/roborts_camera/cmake_module/FindProtoBuf.cmake new file mode 100755 index 00000000..3462e7be --- /dev/null +++ b/roborts_camera/cmake_module/FindProtoBuf.cmake @@ -0,0 +1,84 @@ +# Finds Google Protocol Buffers library and compilers and extends +# the standard cmake script + +find_package(Protobuf REQUIRED) +list(APPEND RRTS_INCLUDE_DIRS PUBLIC ${PROTOBUF_INCLUDE_DIR}) +list(APPEND RRTS_LINKER_LIBS PUBLIC ${PROTOBUF_LIBRARIES}) + +# As of Ubuntu 14.04 protoc is no longer a part of libprotobuf-dev package +# and should be installed separately as in: sudo apt-get install protobuf-compiler +if(EXISTS ${PROTOBUF_PROTOC_EXECUTABLE}) + message(STATUS "Found PROTOBUF Compiler: ${PROTOBUF_PROTOC_EXECUTABLE}") +else() + message(FATAL_ERROR "Could not find PROTOBUF Compiler") +endif() + +if (PROTOBUF_FOUND) + message ("protobuf ${PROTOBUF_LIBRARIES} found") +else () + message (FATAL_ERROR "Cannot find protobuf") +endif () + +# place where to generate protobuf sources +#set(proto_gen_folder "${PROJECT_BINARY_DIR}/include/caffe/proto") +#include_directories("${PROJECT_BINARY_DIR}/include") + +set(PROTOBUF_GENERATE_CPP_APPEND_PATH TRUE) + +################################################################################################ +# Modification of standard 'protobuf_generate_cpp()' with output dir parameter +# Usage: +# caffe_protobuf_generate_cpp_py( ) +function(rrts_protobuf_generate_cpp output_dir proto_srcs proto_hdrs) + if(NOT ARGN) + message(SEND_ERROR "Error: rrts_protobuf_generate_cpp() called without any proto files") + return() + endif() + + if(PROTOBUF_GENERATE_CPP_APPEND_PATH) + # Create an include path for each file specified + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(abs_path ${abs_fil} PATH) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + else() + set(_protoc_include -I ${CMAKE_CURRENT_SOURCE_DIR}) + endif() + + if(DEFINED PROTOBUF_IMPORT_DIRS) + foreach(dir ${PROTOBUF_IMPORT_DIRS}) + get_filename_component(abs_path ${dir} ABSOLUTE) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + endif() + + set(${proto_srcs}) + set(${proto_hdrs}) + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(fil_we ${fil} NAME_WE) + + list(APPEND ${proto_srcs} "${output_dir}/${fil_we}.pb.cc") + list(APPEND ${proto_hdrs} "${output_dir}/${fil_we}.pb.h") + + add_custom_command( + OUTPUT "${output_dir}/${fil_we}.pb.cpp" + "${output_dir}/${fil_we}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${output_dir} ${_protoc_include} ${abs_fil} + DEPENDS ${abs_fil} + COMMENT "Running C++ protocol buffer compiler on ${fil}" + VERBATIM ) + endforeach() + + set_source_files_properties(${${proto_srcs}} ${${proto_hdrs}} PROPERTIES GENERATED TRUE) + set(${proto_srcs} ${${proto_srcs}} PARENT_SCOPE) + set(${proto_hdrs} ${${proto_hdrs}} PARENT_SCOPE) +endfunction() diff --git a/roborts_camera/config/camera_param.prototxt b/roborts_camera/config/camera_param.prototxt new file mode 100755 index 00000000..9df65121 --- /dev/null +++ b/roborts_camera/config/camera_param.prototxt @@ -0,0 +1,41 @@ +camera: { + camera_name: "back_camera" + camera_type: "uvc" + camera_path: "/dev/video0" + + camera_matrix { + data: 839.923052 + data: 0.0 + data: 340.780730 + data: 0.0 + data: 837.671081 + data: 261.766523 + data: 0.0 + data: 0.0 + data: 1.0 + } + + camera_distortion { + data: 0.082613 + data: 0.043275 + data: 0.002486 + data: -0.000823 + data: 0.0 + } + + resolution { + width: 640 + height: 360 + + width_offset: 0 + height_offset: 0 + } + + fps: 100 + auto_exposure: 1 + exposure_value: 70 + exposure_time: 10 + auto_white_balance: 1 + auto_gain: 0 +} + diff --git a/roborts_camera/package.xml b/roborts_camera/package.xml new file mode 100755 index 00000000..5580edc6 --- /dev/null +++ b/roborts_camera/package.xml @@ -0,0 +1,27 @@ + + roborts_camera + 1.0.0 + + The roborts camera package provides camera interface for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + roscpp + rospy + roborts_common + + actionlib + roscpp + rospy + roborts_common + + + + + diff --git a/roborts_camera/proto/camera_param.proto b/roborts_camera/proto/camera_param.proto new file mode 100755 index 00000000..ac5e73ce --- /dev/null +++ b/roborts_camera/proto/camera_param.proto @@ -0,0 +1,43 @@ +syntax = "proto2"; +package roborts_camera; + +message CameraMatrix { + repeated double data = 1; +} + +message CameraDistortion { + repeated double data = 1; +} + +message Resolution { + required uint32 width = 1; + required uint32 height = 2; + + required uint32 width_offset = 3; + required uint32 height_offset = 4; +} + +message Camera { + required string camera_name = 1; + required string camera_type = 2; + required string camera_path = 3; + required CameraMatrix camera_matrix = 4; + required CameraDistortion camera_distortion = 5; + + optional uint32 fps = 6; + optional Resolution resolution = 7; + + optional bool auto_exposure = 8; //1 open, 0 close + optional uint32 exposure_value = 9; + optional uint32 exposure_time = 10;//us + + optional bool auto_white_balance = 11; //1 open, 0 close + + optional bool auto_gain = 12;////1 open, 0 close + + optional uint32 contrast = 13; +} + +message Cameras { + repeated Camera camera = 1; +} diff --git a/roborts_camera/test/image_capture.cpp b/roborts_camera/test/image_capture.cpp new file mode 100644 index 00000000..5d44e199 --- /dev/null +++ b/roborts_camera/test/image_capture.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include +#include +#include +//opencv +#include "opencv2/opencv.hpp" +//ros +#include "image_transport/image_transport.h" +#include "cv_bridge/cv_bridge.h" + +std::string topic_name = "back_camera"; +cv::VideoWriter writer(topic_name+".avi", CV_FOURCC('M', 'J', 'P', 'G'), 25.0, cv::Size(640, 360)); +cv::Mat src_img; + +void ReceiveImg(const sensor_msgs::ImageConstPtr &msg) { + src_img = cv_bridge::toCvShare(msg, "bgr8")->image.clone(); + writer.write(src_img); +} + +int main(int argc, char **argv) { + + ros::init(argc, argv, "image_capture"); + ros::NodeHandle nh; + + image_transport::ImageTransport it(nh); + + image_transport::Subscriber sub = it.subscribe(topic_name, 20, boost::bind(&ReceiveImg, _1)); + + ros::AsyncSpinner async_spinner(1); + async_spinner.start(); + ros::waitForShutdown(); + return 0; +} diff --git a/roborts_camera/uvc/CMakeLists.txt b/roborts_camera/uvc/CMakeLists.txt new file mode 100755 index 00000000..e04fd0dd --- /dev/null +++ b/roborts_camera/uvc/CMakeLists.txt @@ -0,0 +1,21 @@ +project(uvc_driver) + +#uvc_driver +add_library(uvc_driver + SHARED + uvc_driver.cpp +) + +target_link_libraries(uvc_driver + PRIVATE + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +target_include_directories(uvc_driver + PRIVATE + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRECTORIES} +) + +add_library(driver::uvc_driver ALIAS uvc_driver) diff --git a/roborts_camera/uvc/uvc_driver.cpp b/roborts_camera/uvc/uvc_driver.cpp new file mode 100755 index 00000000..b75a82b3 --- /dev/null +++ b/roborts_camera/uvc/uvc_driver.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include +#include + +#include +#include + +#include "uvc_driver.h" + +namespace roborts_camera { +UVCDriver::UVCDriver(CameraInfo camera_info): + CameraBase(camera_info){ +} + +void UVCDriver::StartReadCamera(cv::Mat &img) { + if(!camera_initialized_){ + camera_info_.cap_handle.open(camera_info_.camera_path); + SetCameraExposure(camera_info_.camera_path, camera_info_.exposure_value); + ROS_ASSERT_MSG(camera_info_.cap_handle.isOpened(), "Cannot open %s .", cameras_[camera_num].video_path.c_str()); + camera_initialized_ = true; + } + else { + camera_info_.cap_handle >> img; + } +} + +void UVCDriver::StopReadCamera() { + //TODO: To be implemented +} + +void UVCDriver::SetCameraExposure(std::string id, int val) +{ + int cam_fd; + if ((cam_fd = open(id.c_str(), O_RDWR)) == -1) { + std::cerr << "Camera open error" << std::endl; + } + + struct v4l2_control control_s; + control_s.id = V4L2_CID_AUTO_WHITE_BALANCE; + control_s.value = camera_info_.auto_white_balance; + ioctl(cam_fd, VIDIOC_S_CTRL, &control_s); + + control_s.id = V4L2_CID_EXPOSURE_AUTO; + control_s.value = V4L2_EXPOSURE_MANUAL; + ioctl(cam_fd, VIDIOC_S_CTRL, &control_s); + + // Set exposure value + control_s.id = V4L2_CID_EXPOSURE_ABSOLUTE; + control_s.value = val; + ioctl(cam_fd, VIDIOC_S_CTRL, &control_s); + close(cam_fd); +} + +UVCDriver::~UVCDriver() { +} + +} //namespace roborts_camera diff --git a/roborts_camera/uvc/uvc_driver.h b/roborts_camera/uvc/uvc_driver.h new file mode 100755 index 00000000..c91df196 --- /dev/null +++ b/roborts_camera/uvc/uvc_driver.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_CAMERA_UVC_DRIVER_H +#define ROBORTS_CAMERA_UVC_DRIVER_H + +#include + +#include "ros/ros.h" +#include "opencv2/opencv.hpp" +#include "actionlib/server/simple_action_server.h" + +#include "../camera_param.h" +#include "../camera_base.h" +#include "alg_factory/algorithm_factory.h" +#include "io/io.h" + +namespace roborts_camera { +/** + * @brief UVC Camera class, product of the camera factory inherited from CameraBase + */ +class UVCDriver: public CameraBase { + public: + /** + * @brief Constructor of UVCDriver + * @param camera_info Information and parameters of camera + */ + explicit UVCDriver(CameraInfo camera_info); + /** + * @brief Start to read uvc camera + * @param img Image data in form of cv::Mat to be read + */ + void StartReadCamera(cv::Mat &img) override; + /** + * @brief Stop to read uvc camera + */ + void StopReadCamera(); + ~UVCDriver() override; + private: + /** + * @brief Set camera exposure + * @param id Camera path + * @param val Camera exposure value + */ + void SetCameraExposure(std::string id, int val); + //! Initialization of camera read + bool read_camera_initialized_; +}; + +roborts_common::REGISTER_ALGORITHM(CameraBase, "uvc", UVCDriver, CameraInfo); + +} //namespace roborts_camera +#endif //ROBORTS_CAMERA_UVC_DRIVER_H diff --git a/roborts_common/CMakeLists.txt b/roborts_common/CMakeLists.txt new file mode 100755 index 00000000..e5b4e404 --- /dev/null +++ b/roborts_common/CMakeLists.txt @@ -0,0 +1,29 @@ +project(roborts_common) +cmake_minimum_required(VERSION 3.1) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) + + +find_package(catkin REQUIRED COMPONENTS + roscpp + roborts_msgs + actionlib + ) + +find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS + include +) +include_directories(include) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + diff --git a/roborts_common/cmake_module/FindEigen3.cmake b/roborts_common/cmake_module/FindEigen3.cmake new file mode 100755 index 00000000..239c1bbe --- /dev/null +++ b/roborts_common/cmake_module/FindEigen3.cmake @@ -0,0 +1,86 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/roborts_common/cmake_module/FindGTEST.cmake b/roborts_common/cmake_module/FindGTEST.cmake new file mode 100755 index 00000000..ce7665d0 --- /dev/null +++ b/roborts_common/cmake_module/FindGTEST.cmake @@ -0,0 +1,10 @@ +find_path(GTEST_INCLUDE_DIR gtest.h /usr/include/gtest) +find_library(GTEST_LIBRARY NAMES gtest PATH /usr/lib) +if(GTEST_INCLUDE_DIR AND GTEST_LIBRARY) +set(GTEST_FOUND TRUE) +endif(GTEST_INCLUDE_DIR AND GTEST_LIBRARY) +if(GTEST_FOUND) +if(NOT GTEST_FIND_QUIETLY) +message(STATUS "Found GTEST:" ${GTEST_LIBRARY}) +endif(NOT GTEST_FIND_QUIETLY) +endif(GTEST_FOUND) diff --git a/roborts_common/cmake_module/FindGlog.cmake b/roborts_common/cmake_module/FindGlog.cmake new file mode 100755 index 00000000..99abbe47 --- /dev/null +++ b/roborts_common/cmake_module/FindGlog.cmake @@ -0,0 +1,48 @@ +# - Try to find Glog +# +# The following variables are optionally searched for defaults +# GLOG_ROOT_DIR: Base directory where all GLOG components are found +# +# The following are set after configuration is done: +# GLOG_FOUND +# GLOG_INCLUDE_DIRS +# GLOG_LIBRARIES +# GLOG_LIBRARYRARY_DIRS + +include(FindPackageHandleStandardArgs) + +set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") + +if(WIN32) + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}/src/windows) +else() + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}) +endif() + +if(MSVC) + find_library(GLOG_LIBRARY_RELEASE libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Release) + + find_library(GLOG_LIBRARY_DEBUG libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Debug) + + set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) +else() + find_library(GLOG_LIBRARY glog + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES lib lib64) +endif() + +find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) + +if(GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) + message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") + mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG + GLOG_LIBRARY GLOG_INCLUDE_DIR) +endif() diff --git a/roborts_common/cmake_module/FindProtoBuf.cmake b/roborts_common/cmake_module/FindProtoBuf.cmake new file mode 100755 index 00000000..3462e7be --- /dev/null +++ b/roborts_common/cmake_module/FindProtoBuf.cmake @@ -0,0 +1,84 @@ +# Finds Google Protocol Buffers library and compilers and extends +# the standard cmake script + +find_package(Protobuf REQUIRED) +list(APPEND RRTS_INCLUDE_DIRS PUBLIC ${PROTOBUF_INCLUDE_DIR}) +list(APPEND RRTS_LINKER_LIBS PUBLIC ${PROTOBUF_LIBRARIES}) + +# As of Ubuntu 14.04 protoc is no longer a part of libprotobuf-dev package +# and should be installed separately as in: sudo apt-get install protobuf-compiler +if(EXISTS ${PROTOBUF_PROTOC_EXECUTABLE}) + message(STATUS "Found PROTOBUF Compiler: ${PROTOBUF_PROTOC_EXECUTABLE}") +else() + message(FATAL_ERROR "Could not find PROTOBUF Compiler") +endif() + +if (PROTOBUF_FOUND) + message ("protobuf ${PROTOBUF_LIBRARIES} found") +else () + message (FATAL_ERROR "Cannot find protobuf") +endif () + +# place where to generate protobuf sources +#set(proto_gen_folder "${PROJECT_BINARY_DIR}/include/caffe/proto") +#include_directories("${PROJECT_BINARY_DIR}/include") + +set(PROTOBUF_GENERATE_CPP_APPEND_PATH TRUE) + +################################################################################################ +# Modification of standard 'protobuf_generate_cpp()' with output dir parameter +# Usage: +# caffe_protobuf_generate_cpp_py( ) +function(rrts_protobuf_generate_cpp output_dir proto_srcs proto_hdrs) + if(NOT ARGN) + message(SEND_ERROR "Error: rrts_protobuf_generate_cpp() called without any proto files") + return() + endif() + + if(PROTOBUF_GENERATE_CPP_APPEND_PATH) + # Create an include path for each file specified + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(abs_path ${abs_fil} PATH) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + else() + set(_protoc_include -I ${CMAKE_CURRENT_SOURCE_DIR}) + endif() + + if(DEFINED PROTOBUF_IMPORT_DIRS) + foreach(dir ${PROTOBUF_IMPORT_DIRS}) + get_filename_component(abs_path ${dir} ABSOLUTE) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + endif() + + set(${proto_srcs}) + set(${proto_hdrs}) + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(fil_we ${fil} NAME_WE) + + list(APPEND ${proto_srcs} "${output_dir}/${fil_we}.pb.cc") + list(APPEND ${proto_hdrs} "${output_dir}/${fil_we}.pb.h") + + add_custom_command( + OUTPUT "${output_dir}/${fil_we}.pb.cpp" + "${output_dir}/${fil_we}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${output_dir} ${_protoc_include} ${abs_fil} + DEPENDS ${abs_fil} + COMMENT "Running C++ protocol buffer compiler on ${fil}" + VERBATIM ) + endforeach() + + set_source_files_properties(${${proto_srcs}} ${${proto_hdrs}} PROPERTIES GENERATED TRUE) + set(${proto_srcs} ${${proto_srcs}} PARENT_SCOPE) + set(${proto_hdrs} ${${proto_hdrs}} PARENT_SCOPE) +endfunction() diff --git a/roborts_common/include/alg_factory/CMakeLists.txt b/roborts_common/include/alg_factory/CMakeLists.txt new file mode 100644 index 00000000..59b83757 --- /dev/null +++ b/roborts_common/include/alg_factory/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.1) +project(alg_factory) + +#common::io +add_library(alg_factory INTERFACE) + +target_include_directories(alg_factory INTERFACE include/) + +#add_library(roborts_common::alg_factory ALIAS alg_factory) \ No newline at end of file diff --git a/roborts_common/include/alg_factory/algorithm_factory.h b/roborts_common/include/alg_factory/algorithm_factory.h new file mode 100755 index 00000000..f812ed8c --- /dev/null +++ b/roborts_common/include/alg_factory/algorithm_factory.h @@ -0,0 +1,192 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_COMMON_ALGORITHM_FACTORY_H +#define ROBORTS_COMMON_ALGORITHM_FACTORY_H +#include +#include +#include +#include +#include + +#include "bind_this.h" + +namespace roborts_common{ +/** + * @brief In order to conveniently using the sepecific algorithm in each function module + * such as mapping, localization, planning, it is nessessary to register algorithm + * class to the algorithm container in AlgorithmFactory class. The example is at + * the end of this program. + * @tparam AlgorithmBase The base class of algorithm. + * @tparam Args Parameter pack + */ +template +class AlgorithmFactory +{ + public: + using AlgorithmHash = std::unordered_map(Args...)>>; + + /** + * @brief The only way to get AlgorithmHash. + * @return Ref of AlgorithmHash + */ + static AlgorithmHash& GetAlgorithmHash(){ + static AlgorithmHash algorithm_hash; + return algorithm_hash; + } + + /** + * @brief Register specific algorithm to the algorithm container called AlgorithmHash. + * @tparam ParamType Type emplate of parameters + * @param algorithm_name Algorithm name, which must be the same with the variable defined in ".prototxt" file. + * @param args Parameter pack + * @return Return true if register successfully, false otherwith. + */ + template + static bool Register(const std::string algorithm_name, + ParamType&& args){ + AlgorithmHash& algorithm_hash = GetAlgorithmHash(); + auto factory_iter = algorithm_hash.find(algorithm_name); + if(factory_iter == algorithm_hash.end()) { + algorithm_hash.emplace(std::make_pair(algorithm_name, std::forward(args))); + std::cout << algorithm_name << " registered successfully!" << std::endl; + } + else + std::cout << algorithm_name << " has been registered!" << std::endl; + return true; + } + + /** + * @brief Unregister algorithm from the algorithm container. + * @param algorithm_name Algorithm name, which must be the same with the variable defined in ".prototxt" file. + * @return Return true if register successfully, false otherwith. + */ + static bool UnRegister(const std::string algorithm_name){ + AlgorithmHash& algorithm_hash = GetAlgorithmHash(); + auto factory_iter = algorithm_hash.find(algorithm_name); + if(factory_iter != algorithm_hash.end()){ + algorithm_hash.erase(algorithm_name); + return true; + } + else{ + std::cout << "Failed to unregister algorithm, it is a unregistered alrorithm." + << algorithm_name << std::endl; + return false; + } + } + + /** + * @brief Create an algorithm class that has been registered before. + * @param algorithm_name Algorithm name, which must be the same with the variable defined in ".prototxt" file. + * @param args Parameter pack + * @return The base class unique_ptr that point to Algorithm corresponding to the algorithm name. + */ + static std::unique_ptr CreateAlgorithm(const std::string algorithm_name, + Args... args) { + AlgorithmHash& algorithm_hash = GetAlgorithmHash(); + + auto factory_iter = algorithm_hash.find(algorithm_name); + if(factory_iter == algorithm_hash.end()){ + std::cout << "Can't creat algorithm " << algorithm_name << + ", because you haven't register it!" << std::endl; + return nullptr; + } + else{ + return (factory_iter->second)(std::forward(args)...); + } + } + + private: + AlgorithmFactory(){} +}; + +template +class AlgorithmRegister{ + public: + /** + * @brief Constructor function of AlgorithmRegister class. + * @param algorithm_name Algorithm name, which must be the same with the variable defined in ".prototxt" file. + */ + explicit AlgorithmRegister(std::string algorithm_name) { + Register(algorithm_name, make_int_sequence{}); + } + /** + * @brief Create an unique_ptr pointer of Algorithm corresponding to the algorithm name. + * @param data Parameter pack + * @return The base class unique_ptr that point to Algorithm corresponding to the algorithm name. + */ + static std::unique_ptr create(Args&&... data) + { + return std::make_unique(std::forward(data)...); + } + + private: + /** + * @brief Using std::bind to create a std::function, than pass it to Register function befined in AlgorithmnFactory + * class + * @tparam Is Parameter pack + * @param algorithm_name Algorithm name, which must be the same with the variable defined in ".prototxt" file. + */ + template + void Register(std::string algorithm_name, int_sequence) { + auto function = std::bind(&AlgorithmRegister::create, + std::placeholder_template{}...); + AlgorithmFactory::Register(algorithm_name, function); + } +}; +/** + * @brief It is convenient to register specific algorithm using this Macro. + * @example + * class BaseClass { + * public: + * BaseClass(int num){ + * num_ = num; + * } + * virtual void Excute() = 0; + * public: + * int num_; + * }; + * + * class A: public BaseClass { + * public: + * A(int num): BaseClass(num) {} + * void Excute(){ + * std::cout << "A, you are so beautiful!" << std::endl; + * } + * }; + * REGISTER_ALGORITHM(BaseClass, "a", A, int); + * + * class B: public BaseClass { + * public: + * B(int num): BaseClass(num){} + * void Excute() { + * std::cout << "B, you are so beautiful!" << std::endl; + * } + * }; + * REGISTER_ALGORITHM(BaseClass, "b", B, int); + * + * int main() { + * auto base_class = AlgorithmFactory::CreateAlgorithm("a", 3); + * base_class->Excute(); + * } + */ +#define NAME(name) register_##name##_algorithm +#define REGISTER_ALGORITHM(AlgorithmBase, AlgorithmName, Algorithm, ...) \ + AlgorithmRegister NAME(Algorithm)(AlgorithmName) +} //namespace roborts_common + +#endif // ROBORTS_COMMON_ALGORITHM_FACTORY_H \ No newline at end of file diff --git a/roborts_common/include/alg_factory/bind_this.h b/roborts_common/include/alg_factory/bind_this.h new file mode 100755 index 00000000..3aa37408 --- /dev/null +++ b/roborts_common/include/alg_factory/bind_this.h @@ -0,0 +1,60 @@ +#ifndef RRTS_BIND_THIS_H +#define RRTS_BIND_THIS_H +namespace std +{ +template // begin with 0 here! +struct placeholder_template +{}; + +template +struct is_placeholder > + : integral_constant // the one is important +{}; +} // end of namespace std + + +template +struct int_sequence +{}; + +template +struct make_int_sequence + : make_int_sequence +{}; + +template +struct make_int_sequence<0, Is...> + : int_sequence +{}; + +template +auto bind_this_sub(R (U::*p)(Args...), U * pp, int_sequence) +-> decltype(std::bind(p, pp, std::placeholder_template{}...)) +{ + return std::bind(p, pp, std::placeholder_template{}...); +} + +// binds a member function only for the this pointer using std::bind +template +auto bind_this(R (U::*p)(Args...), U * pp) +-> decltype(bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{})) +{ + return bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{}); +} + +// utility +template +auto bind_this_sub(R (U::*p)(Args...) const, U * pp, int_sequence) +-> decltype(std::bind(p, pp, std::placeholder_template{}...)) +{ + return std::bind(p, pp, std::placeholder_template{}...); +} + +// binds a member function only for the this pointer using std::bind +template +auto bind_this(R (U::*p)(Args...) const, U * pp) +-> decltype(bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{})) +{ + return bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{}); +} +#endif //RRTS_BIND_THIS_H diff --git a/roborts_common/include/io/CMakeLists.txt b/roborts_common/include/io/CMakeLists.txt new file mode 100644 index 00000000..ce02612b --- /dev/null +++ b/roborts_common/include/io/CMakeLists.txt @@ -0,0 +1,7 @@ +project(io) + +#common::io +add_library(io INTERFACE) + +target_sources(io INTERFACE io.h) +target_link_libraries(io INTERFACE ${PROTOBUF_LIBRARIES}) diff --git a/roborts_common/include/io/io.h b/roborts_common/include/io/io.h new file mode 100755 index 00000000..84edd48d --- /dev/null +++ b/roborts_common/include/io/io.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_COMMON_IO_H +#define ROBORTS_COMMON_IO_H +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + + +namespace roborts_common{ +const int kProtoReadBytesLimit = INT_MAX; // Max size of 2 GB minus 1 byte. + +template +inline bool ReadProtoFromTextFile(const char *file_name, T *proto) { + using google::protobuf::io::FileInputStream; + using google::protobuf::io::FileOutputStream; + using google::protobuf::io::ZeroCopyInputStream; + using google::protobuf::io::CodedInputStream; + using google::protobuf::io::ZeroCopyOutputStream; + using google::protobuf::io::CodedOutputStream; + using google::protobuf::Message; + + std::string full_path = /*ros::package::getPath("roborts") +*/ std::string(file_name); + ROS_INFO("Load prototxt: %s", full_path.c_str()); + + int fd = open(full_path.c_str(), O_RDONLY); + if (fd == -1) { + ROS_ERROR("File not found: %s", full_path.c_str()); + return false; + } + FileInputStream *input = new FileInputStream(fd); + bool success = google::protobuf::TextFormat::Parse(input, proto); + delete input; + close(fd); + return success; +} + +template +inline bool ReadProtoFromTextFile(const std::string &file_name, T *proto) { + return ReadProtoFromTextFile(file_name.c_str(), proto); +} + +template +inline bool ReadProtoFromBinaryFile(const char *file_name, T *proto) { + using google::protobuf::io::FileInputStream; + using google::protobuf::io::FileOutputStream; + using google::protobuf::io::ZeroCopyInputStream; + using google::protobuf::io::CodedInputStream; + using google::protobuf::io::ZeroCopyOutputStream; + using google::protobuf::io::CodedOutputStream; + using google::protobuf::Message; + + int fd = open(file_name, O_RDONLY); + if (fd == -1) { + proto = NULL; + ROS_ERROR("File not found: %s", file_name); + } + + ZeroCopyInputStream *raw_input = new FileInputStream(fd); + CodedInputStream *coded_input = new CodedInputStream(raw_input); + coded_input->SetTotalBytesLimit(kProtoReadBytesLimit, 536870912); + + bool success = proto->ParseFromCodedStream(coded_input); + + delete coded_input; + delete raw_input; + close(fd); + return success; +} +template +inline bool ReadProtoFromBinaryFile(const std::string &file_name, T *proto) { + return ReadProtoFromBinaryFile(file_name.c_str(), proto); +} + +template +inline bool ReadYmlFromFile(const char *file_name, T *yml_type); +template +inline bool ReadYmlFromFile(const std::string &file_name, T *yml_type) { + return ReadYmlFromFile(file_name.c_str(), yml_type); +} +} + +#endif // ROBORTS_COMMON_IO_H \ No newline at end of file diff --git a/roborts_common/include/state/CMakeLists.txt b/roborts_common/include/state/CMakeLists.txt new file mode 100644 index 00000000..29646899 --- /dev/null +++ b/roborts_common/include/state/CMakeLists.txt @@ -0,0 +1,6 @@ +project(roborts_state) + +#common::io +add_library(roborts_state INTERFACE) + +target_include_directories(roborts_state INTERFACE include/) diff --git a/roborts_common/include/state/error_code.h b/roborts_common/include/state/error_code.h new file mode 100755 index 00000000..629d2a11 --- /dev/null +++ b/roborts_common/include/state/error_code.h @@ -0,0 +1,131 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_COMMON_ERROR_CODE_H +#define ROBORTS_COMMON_ERROR_CODE_H + +#include + +namespace roborts_common{ + +enum ErrorCode{ + OK = 0, + Error = 1, +/************************HARDWARE********************/ + + + +/***********************SOFTWARRE********************/ + /***************DRIVER******************/ + //camera + CAMERA_ERROR = 10000, + IMAGE_READ_ERROR = 10001, + STOP_DETECTION = 10002, + + //lidar + LIDAR_ERROR = 10100, + + + /**************PERCEPTION***************/ + //mapping + MAPPING_ERROR = 11000, + + //map + MAP_ERROR = 12000, + + //localization + LOCALIZATION_INIT_ERROR = 12100, + + //detection + DETECTION_INIT_ERROR = 12200, + + + /**************DECISION*****************/ + //decision + DECISION_ERROR = 13000, + + + /**************PLANNING*****************/ + + //global planner + GP_INITILIZATION_ERROR = 14000, + GP_GET_POSE_ERROR, + GP_POSE_TRANSFORM_ERROR, + GP_GOAL_INVALID_ERROR, + GP_PATH_SEARCH_ERROR, + GP_MOVE_COST_ERROR, + GP_MAX_RETRIES_FAILURE, + GP_TIME_OUT_ERROR, + + + //local planner + LP_PLANNING_ERROR = 14100, + LP_INITILIZATION_ERROR = 14101, + LP_ALGORITHM_INITILIZATION_ERROR = 14102, + LP_ALGORITHM_TRAJECTORY_ERROR= 14103, + LP_ALGORITHM_GOAL_REACHED= 14104, + LP_MAX_ERROR_FAILURE = 14105, + LP_PLANTRANSFORM_ERROR = 14106, + LP_OPTIMAL_ERROR = 14107, + LP_VELOCITY_ERROR = 14108, + LP_OSCILLATION_ERROR + + + /**************CONTROL******************/ +}; + +class ErrorInfo { + + public: + ErrorInfo():error_code_(ErrorCode::OK),error_msg_(""){}; + ErrorInfo(ErrorCode error_code, const std::string &error_msg=""):error_code_(error_code),error_msg_(error_msg){}; + + ErrorInfo& operator= ( const ErrorInfo& error_info ) { + if (&error_info != this) { + error_code_ = error_info.error_code_; + error_msg_ = error_info.error_msg_; + } + return *this; + } + + static ErrorInfo OK(){ + return ErrorInfo(); + } + + ErrorCode error_code() const { return error_code_;}; + const std::string &error_msg() const { return error_msg_; } + + bool operator==(const ErrorInfo& rhs){ + return error_code_==rhs.error_code(); + } + + bool IsOK() const{ + return (error_code_ == ErrorCode::OK); + } + + ~ErrorInfo()= default; + + private: + ErrorCode error_code_; + std::string error_msg_; + + +}; + +} //namespace roborts_common + +#endif //ROBORTS_COMMON_ERROR_CODE_H diff --git a/roborts_common/include/state/node_state.h b/roborts_common/include/state/node_state.h new file mode 100755 index 00000000..10deebd6 --- /dev/null +++ b/roborts_common/include/state/node_state.h @@ -0,0 +1,31 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef RRTS_NODE_STATE_H +#define RRTS_NODE_STATE_H +namespace roborts_common{ + +enum NodeState{ + IDLE, + RUNNING, + PAUSE, + SUCCESS, + FAILURE +}; +} //namespace roborts_common + +#endif //RRTS_NODE_STATE_H diff --git a/roborts_common/include/timer/CMakeLists.txt b/roborts_common/include/timer/CMakeLists.txt new file mode 100644 index 00000000..165798ab --- /dev/null +++ b/roborts_common/include/timer/CMakeLists.txt @@ -0,0 +1,6 @@ +project(roborts_timer) + +#common::io +add_library(roborts_timer INTERFACE) + +target_include_directories(roborts_timer INTERFACE include/) \ No newline at end of file diff --git a/roborts_common/include/timer/timer.h b/roborts_common/include/timer/timer.h new file mode 100755 index 00000000..19255cf1 --- /dev/null +++ b/roborts_common/include/timer/timer.h @@ -0,0 +1,27 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef TIMER_H +#define TIMER_H +#include +#include + + +#define TIMER_START(FUNC) boost::timer t_##FUNC; +#define TIMER_END(FUNC) std::cout << "[" << #FUNC << "]" << "cost time: " << t_##FUNC.elapsed() << std::endl; + +#endif // TIMER_H diff --git a/roborts_common/math/CMakeLists.txt b/roborts_common/math/CMakeLists.txt new file mode 100644 index 00000000..28d626b4 --- /dev/null +++ b/roborts_common/math/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.1) +project(roborts_math) + +#common::math +add_library(roborts_math INTERFACE + +) + +target_sources(roborts_math INTERFACE + geometry.h + math.h +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + ${EIGEN3_INCLUDE_DIR} + ) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + ${EIGEN3_LIBRARIES} + ${catkin_LIBRARIES} + ${GLOG_LIBRARY} + ) diff --git a/roborts_common/math/geometry.h b/roborts_common/math/geometry.h new file mode 100644 index 00000000..92ef2b51 --- /dev/null +++ b/roborts_common/math/geometry.h @@ -0,0 +1,249 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_COMMON_GEOMETRY_H +#define ROBORTS_COMMON_GEOMETRY_H + +#include +#include +#include + +namespace roborts_common { + +constexpr double kEpsilon = 1e-10; + +class Point2D { + public: + + constexpr Point2D(const double x, const double y) noexcept : x_(x), y_(y), angle_(std::atan2(y_, x_)) {} + + constexpr Point2D() noexcept : Point2D(0, 0) {} + + double X() const { + return x_; + } + + double Y() const { + return y_; + } + + double Angle() const { + return angle_; + } + + void SetX(const double x) { + x_ = x; + } + + void SetY(const double y) { + y_ = y; + } + + void Normalize() { + x_ = cos(angle_); + y_ = sin(angle_); + } + + Point2D Rotate(const double angle) const { + double x = x_ * cos(angle) - y_ * sin(angle); + double y = x_ * sin(angle) + y_ * cos(angle); + return Point2D(x, y); + } + + friend Point2D operator+(const Point2D &point_a, const Point2D &point_b) { + double x = point_a.X() + point_b.X(); + double y = point_a.Y() + point_b.Y(); + return Point2D(x, y); + } + + friend Point2D operator-(const Point2D &point_a, const Point2D &point_b) { + double x = point_a.X() - point_b.X(); + double y = point_a.Y() - point_b.Y(); + return Point2D(x, y); + }; + + Point2D operator*(const double ratio) const { + return Point2D(x_ * ratio, y_ * ratio); + } + + Point2D &operator+=(const Point2D &rhs) { + x_ += rhs.X(); + y_ += rhs.Y(); + return *this; + } + + Point2D &operator-=(const Point2D &rhs) { + x_ -= rhs.X(); + y_ -= rhs.Y(); + return *this; + }; + + Point2D &operator*=(const double ratio) { + x_ *= ratio; + y_ *= ratio; + return *this; + }; + + friend bool operator==(const Point2D &point_a, const Point2D &point_b) { + return (std::abs(point_a.X() - point_b.X()) < kEpsilon && + std::abs(point_a.Y() - point_b.Y()) < kEpsilon); + }; + + protected: + double x_; + double y_; + double angle_; +}; + +class LineSegment2D { + + public: + LineSegment2D(); + + LineSegment2D(const Point2D &start, const Point2D &end) : start_(start), end_(end) { + dx_ = end_.X() - start_.X(); + dy_ = end_.Y() - start_.Y(); + length_ = hypot(dx_, dy_); + unit_direction_ = Point2D(dx_ / length_, dy_ / length_); + } + + const Point2D &Start() const { + return start_; + } + + const Point2D &End() const { + return end_; + } + + const double &DiffX() const { + return dx_; + } + + const double &DiffY() const { + return dy_; + } + + const Point2D &UnitDirection() const { + return unit_direction_; + } + + Point2D Center() const { + return (start_ + end_) * 0.5; + } + + private: + Point2D start_; + Point2D end_; + Point2D unit_direction_; + double length_; + double dx_; + double dy_; +}; + +class Polygon2D { + public: + + Polygon2D() = default; + + explicit Polygon2D(std::vector points) : points_(std::move(points)) { + BuildFromPoints(); + } + + const std::vector &Points() const { + return points_; + } + + const std::vector &Lines() const { + return line_segments_; + } + + int NumPoints() const { + return num_points_; + } + + bool IsConvex() const { + return is_convex_; + } + + double Area() const { + return area_; + } + + double MinX() const { return min_x_; } + double MaxX() const { return max_x_; } + double MinY() const { return min_y_; } + double MaxY() const { return max_y_; } + + protected: + void BuildFromPoints() { + + num_points_ = points_.size(); + line_segments_.reserve(num_points_); + + for (int i = 1; i < num_points_; ++i) { + auto vector0 = points_[i - 1] - points_[0]; + auto vector1 = points_[i] - points_[0]; + area_ += vector0.X() * vector1.Y() - vector0.Y() * vector1.X(); + } + if (area_ < 0) { + std::reverse(points_.begin(), points_.end()); + } + area_ = area_ / 2.0; + + // Construct line_segments. + line_segments_.reserve(num_points_); + for (int i = 0; i < num_points_; ++i) { + line_segments_.emplace_back(points_[i], points_[Next(i)]); + } + + //use cross judge convex + is_convex_ = true; + for (int i = 0; i < num_points_; ++i) { + + auto vector0 = points_[i] - points_[Prev(i)]; + auto vector1 = points_[Next(i)] - points_[Prev(i)]; + + if ((vector0.X() * vector1.Y() - vector0.Y() * vector1.X()) <= -kEpsilon) { + is_convex_ = false; + break; + } + } + } + int Next(int index) const { + return index >= (num_points_ - 1) ? 0 : (index + 1); + } + + int Prev(int index) const { + return index == 0 ? (num_points_ - 1) : (index - 1); + } + + static bool ClipConvexHull(const LineSegment2D &line_segment, + std::vector *const points); + + std::vector points_; + int num_points_ = 0; + std::vector line_segments_; + bool is_convex_ = false; + double area_ = 0.0; + double min_x_ = 0.0; + double max_x_ = 0.0; + double min_y_ = 0.0; + double max_y_ = 0.0; +}; + +} // namespace roborts_common +#endif //ROBORTS_COMMON_GEOMETRY_H diff --git a/roborts_common/math/math.h b/roborts_common/math/math.h new file mode 100644 index 00000000..2350c7c7 --- /dev/null +++ b/roborts_common/math/math.h @@ -0,0 +1,434 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_COMMON_MATH_H +#define ROBORTS_COMMON_MATH_H + +#include +#include + +#include + +#include "geometry.h" +namespace roborts_common { + +inline double Cross2D (const geometry::Point2D &vector0, + const geometry::Point2D &vector1) { + return vector0.X() * vector1.Y() - vector0.Y() * vector1.X(); +} + +inline double Cross2D (const geometry::Point2D &start_point, + const geometry::Point2D &point0, + const geometry::Point2D &point1) { + auto vector0 = point0 - start_point; + auto vector1 = point1 - start_point; + return Cross2D(vector0, vector1); +} + +inline double Cross2D (const Eigen::Vector2d v1, + const Eigen::Vector2d v2) { + return v1.x()*v2.y() - v2.x()*v1.y(); +} + +inline double Cross2D(const double x0, const double y0, + const double x1, const double y1) { + return x0 * y1 - x1 * y0; +} + +inline bool ConvexVerify(std::vector &points) { + + auto num_points = points.size(); + if (num_points < 4) { + return true; + } + + //make sure the point range is Anti-Clock + double area = 0; + for (int i = 1; i < num_points; ++i) { + area += Cross2D(points[0], points[i - 1], points[i]); + } + if (area < 0) { + std::reverse(points.begin(), points.end()); + } + +//use cross judge convex + for (int i = 0; i < num_points; ++i) { + + auto prev = i == 0 ? (num_points - 1) : (i - 1); + auto next = i >= (num_points - 1) ? 0 : (i + 1); + + if (Cross2D(points[prev], points[i], points[next]) <= 1e-10) { + return false; + } + } + + return true; +} + +inline double LogisticSigmoid (const double x) { + return x / (1 + fabs(x)); +} + +inline double PointDistance (const double x0, const double y0, + const double x1, const double y1) { + return hypot(x1 - x0, y1 - y0); +} + +inline double PointDistance (const geometry::Point2D &point0, + const geometry::Point2D &point1) { + return hypot((point1 - point0).X(), (point1 - point0).Y()); +} + +inline double PointDistance (const Eigen::Vector2d &vector0, + const Eigen::Vector2d &vector1) { + return hypot(vector1.x() - vector0.x(), vector1.y() - vector0.y()); +} + +inline double PointToLineDistance (const double px, const double py, + const double x0, const double y0, + const double x1, const double y1) { + double A = px - x0; + double B = py - y0; + double C = x1 - x0; + double D = y1 - y0; + + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; + + double xx, yy; + + if (param < 0) + { + xx = x0; + yy = y0; + } + else if (param > 1) + { + xx = x1; + yy = y1; + } + else + { + xx = x0 + param * C; + yy = y0 + param * D; + } + + return PointDistance(px, py, xx, yy); +} + +inline double PointToLineDistance (const geometry::Point2D &point, + const geometry::LineSegment2D &line) { + double A = point.X() - line.Start().X(); + double B = point.Y() - line.Start().Y(); + double C = line.DiffX(); + double D = line.DiffY(); + + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; + + double xx, yy; + + if (param < 0) + { + xx = line.Start().X(); + yy = line.Start().Y(); + } + else if (param > 1) + { + xx = line.End().X(); + yy = line.End().Y(); + } + else + { + xx = line.Start().X() + param * C; + yy = line.Start().Y() + param * D; + } + + return PointDistance(point.X(), point.Y(), xx, yy); +} + +inline double PointToLineDistance(const Eigen::Ref &point, + const Eigen::Ref &line_start, + const Eigen::Ref &line_end) { + Eigen::Vector2d diff = line_end - line_start; + double sq_norm = diff.squaredNorm(); + + if (sq_norm == 0){ + return (point - line_start).norm(); + } + + double u = ((point.x() - line_start.x()) * diff.coeffRef(0) + (point.y() - line_start.y()) * diff.coeffRef(1)) / sq_norm; + + if (u <= 0) { + return (point - line_start).norm(); + } else if (u >= 1) { + return (point - line_end).norm(); + } + + return PointDistance(point, line_start + u * diff); +} + +inline bool CheckLineSegmentsIntersection2D(const Eigen::Ref &line1_start, + const Eigen::Ref &line1_end, + const Eigen::Ref &line2_start, + const Eigen::Ref &line2_end, + Eigen::Vector2d *intersection = NULL) { + double s_numer, t_numer, denom, t; + Eigen::Vector2d line1 = line1_end - line1_start; + Eigen::Vector2d line2 = line2_end - line2_start; + + denom = line1.coeffRef(0) * line2.coeffRef(1) - line2.coeffRef(0) * line1.coeffRef(1); + if (denom == 0) { + return false; + } + bool denomPositive = denom > 0; + + Eigen::Vector2d aux = line1_start - line2_start; + + s_numer = line1.coeffRef(0) * aux.coeffRef(1) - line1.coeff(1) * aux.coeffRef(0); + if ((s_numer < 0) == denomPositive) { + return false; + } + + t_numer = line2.coeffRef(0) * aux.coeffRef(1) - line2.coeffRef(1) * aux.coeffRef(0); + if ((t_numer < 0) == denomPositive) { + return false; + } + + if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) { + return false; + } + + t = t_numer / denom; + if (intersection) { + *intersection = line1_start + t * line1; + } + + return true; +} + +inline bool CheckLineSegmentsIntersection2D(const geometry::LineSegment2D &line0, + const geometry::LineSegment2D &line1, + geometry::Point2D *intersection = NULL) { + double s_numer, t_numer, denom, t; + + denom = line0.DiffX() * line1.DiffY() - line1.DiffX() * line0.DiffY(); + if (denom == 0) { + return false; + } + bool denomPositive = denom > 0; + + geometry::LineSegment2D aux(line1.Start(), line0.Start()); + + s_numer = line0.DiffX() * aux.DiffY() - line0.DiffY() * aux.DiffX(); + if ((s_numer < 0) == denomPositive) { + return false; + } + + t_numer = line1.DiffX() * aux.DiffY() - line1.DiffY() * aux.DiffX(); + if ((t_numer < 0) == denomPositive) { + return false; + } + + if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) { + return false; + } + + t = t_numer / denom; + if (intersection) { + *intersection = line0.Start() + geometry::Point2D(t * line0.DiffX(), t * line0.DiffY()); + } + + return true; +} + +inline double DistanceSegmentToSegment2D(const Eigen::Ref &line1_start, + const Eigen::Ref &line1_end, + const Eigen::Ref &line2_start, + const Eigen::Ref &line2_end) { + + if (CheckLineSegmentsIntersection2D(line1_start, line1_end, line2_start, line2_end)){ + return 0; + } + + std::array distances; + + distances[0] = PointToLineDistance(line1_start, line2_start, line2_end); + distances[1] = PointToLineDistance(line1_end, line2_start, line2_end); + distances[2] = PointToLineDistance(line2_start, line1_start, line1_end); + distances[3] = PointToLineDistance(line2_end, line1_start, line1_end); + + return *std::min_element(distances.begin(), distances.end()); +} + +inline double DistanceSegmentToSegment2D(const geometry::LineSegment2D &line0, const geometry::LineSegment2D &line1) { + + if (CheckLineSegmentsIntersection2D(line0, line1)) { + return 0; + } + + std::array distances; + + distances[0] = PointToLineDistance(line0.Start(), line1); + distances[1] = PointToLineDistance(line0.End(), line1); + distances[2] = PointToLineDistance(line1.Start(), line0); + distances[3] = PointToLineDistance(line1.End(), line0); + + return *std::min_element(distances.begin(), distances.end()); +} + + +inline double DistancePointToPolygon2D(const Eigen::Vector2d &point, const std::vector &vertices) { + double dist = HUGE_VAL; + + if (vertices.size() == 1) { + return (point - vertices.front()).norm(); + } + + for (int i = 0; i < (int) vertices.size() - 1; ++i) { + + double new_dist = PointToLineDistance(point.coeffRef(0), point.coeffRef(1), vertices.at(i).coeffRef(0), vertices.at(i).coeffRef(1), + vertices.at(i + 1).coeffRef(0), vertices.at(i + 1).coeffRef(1)); + if (new_dist < dist) { + dist = new_dist; + } + + } + + if (vertices.size() > 2) { + double new_dist = PointToLineDistance(point.coeffRef(0), point.coeffRef(1), vertices.back().coeffRef(0), vertices.back().coeffRef(1), + vertices.front().coeffRef(0), vertices.front().coeffRef(1)); + if (new_dist < dist) { + std::cout << "here" << std::endl; + dist = new_dist; + } + } + + return dist; +} + +inline double DistancePointToPolygon2D(const geometry::Point2D &point, const geometry::Polygon2D &polygon) { + double dist = HUGE_VAL; + + if (polygon.NumPoints() == 1) { + return PointDistance(point, polygon.Points().front()); + } + + for (int i = 0; i < (int) polygon.Lines().size() - 1; ++i) { + + double new_dist = PointToLineDistance(point, polygon.Lines().at(i)); + if (new_dist < dist) { + dist = new_dist; + } + + } + + return dist; +} + +inline double DistanceSegmentToPolygon2D(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + const std::vector &vertices) { + double dist = HUGE_VAL; + + if (vertices.size() == 1) { + return PointToLineDistance(vertices.front(), line_start, line_end); + } + + for (int i = 0; i < (int) vertices.size() - 1; ++i) { + double new_dist = DistanceSegmentToSegment2D(line_start, line_end, vertices.at(i), vertices.at(i + 1)); + if (new_dist < dist) { + dist = new_dist; + } + } + + if (vertices.size() > 2) { + double new_dist = + DistanceSegmentToSegment2D(line_start, line_end, vertices.back(), vertices.front()); + if (new_dist < dist) + return new_dist; + } + + return dist; +} + +inline double DistanceSegmentToPolygon2D(const geometry::LineSegment2D line, + const geometry::Polygon2D polygon) { + double dist = HUGE_VAL; + + if (polygon.Points().size() == 1) { + return PointToLineDistance(polygon.Points().front(), line); + } + + for (int i = 0; i < (int) (polygon.Lines().size() - 1); ++i) { + double new_dist = DistanceSegmentToSegment2D(line, polygon.Lines().at(i)); + if (new_dist < dist) { + dist = new_dist; + } + } + + return dist; +} + +inline double DistancePolygonToPolygon2D(const std::vector &vertices1, + const std::vector &vertices2) { + double dist = HUGE_VAL; + + if (vertices1.size() == 1) { + return DistancePointToPolygon2D(vertices1.front(), vertices2); + } + + for (int i = 0; i < (int) vertices1.size() - 1; ++i) { + double new_dist = DistanceSegmentToPolygon2D(vertices1[i], vertices1[i + 1], vertices2); + if (new_dist < dist) { + dist = new_dist; + } + } + + if (vertices1.size() > 2) { + double new_dist = DistanceSegmentToPolygon2D(vertices1.back(), vertices1.front(), vertices2); + if (new_dist < dist) { + return new_dist; + } + } + + return dist; +} + +inline double DistancePolygonToPolygon2D(const geometry::Polygon2D &polygon0, + const geometry::Polygon2D &polygon1) { + double dist = HUGE_VAL; + + if (polygon0.Points().size() == 1) { + return DistancePointToPolygon2D(polygon0.Points().front(), polygon1); + } + + for (int i = 0; i < (int) (polygon0.Lines().size() - 1); ++i) { + double new_dist = DistanceSegmentToPolygon2D(polygon0.Lines().at(i), polygon1); + if (new_dist < dist) { + dist = new_dist; + } + } + + return dist; +} + +} // namespace roborts_common + +#endif //ROBORTS_COMMON_MATH_H diff --git a/roborts_common/package.xml b/roborts_common/package.xml new file mode 100755 index 00000000..5162d4f4 --- /dev/null +++ b/roborts_common/package.xml @@ -0,0 +1,28 @@ + + roborts_common + 1.0.0 + + The roborts common package provides all the common functions for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + roscpp + roborts_msgs + rospy + + actionlib + roscpp + roborts_msgs + rospy + + + + + + diff --git a/roborts_costmap/CMakeLists.txt b/roborts_costmap/CMakeLists.txt new file mode 100755 index 00000000..1ac48e02 --- /dev/null +++ b/roborts_costmap/CMakeLists.txt @@ -0,0 +1,85 @@ +project(roborts_costmap) +cmake_minimum_required(VERSION 3.1) +set(CMAKE_CXX_STANDARD 14) + +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +find_package(Eigen3 REQUIRED) +find_package(ProtoBuf REQUIRED) +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + pcl_conversions + pcl_ros + roborts_common + laser_geometry + sensor_msgs + geometry_msgs + ) + +find_package(PCL 1.7 REQUIRED) +find_package(Boost REQUIRED) + +catkin_package( + INCLUDE_DIRS + include +) + + +#proto file +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") + +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto + CostmapProtoSrcs + CostmapProtoHeaders + ${ProtoFiles} + ) + +include_directories(include/costmap proto/) +aux_source_directory(src/. SRC_LIST) +list(REMOVE_ITEM SRC_LIST "src/test_costmap.cpp") + +#lib project +add_library(roborts_costmap + SHARED + ${SRC_LIST} + ${CostmapProtoHeaders} + ${CostmapProtoSrcs} + ) + +target_include_directories(roborts_costmap + PUBLIC + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ) + +target_link_libraries(roborts_costmap + PUBLIC + ${catkin_LIBRARIES} + ${PROTOBUF_LIBRARIES} + ) + +#test project +add_executable(test_costmap src/test_costmap.cpp) + +target_include_directories(test_costmap + PUBLIC + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ) + +target_link_libraries(test_costmap + roborts_costmap + ${catkin_LIBRARIES} + ${PROTOBUF_LIBRARIES} + ) + +list(APPEND catkin_LIBRARIES roborts_costmap) + +install(DIRECTORY include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + + diff --git a/roborts_costmap/cmake_module/FindEigen3.cmake b/roborts_costmap/cmake_module/FindEigen3.cmake new file mode 100755 index 00000000..239c1bbe --- /dev/null +++ b/roborts_costmap/cmake_module/FindEigen3.cmake @@ -0,0 +1,86 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/roborts_costmap/cmake_module/FindProtoBuf.cmake b/roborts_costmap/cmake_module/FindProtoBuf.cmake new file mode 100755 index 00000000..aee8e000 --- /dev/null +++ b/roborts_costmap/cmake_module/FindProtoBuf.cmake @@ -0,0 +1,84 @@ +# Finds Google Protocol Buffers library and compilers and extends +# the standard cmake script + +find_package(Protobuf REQUIRED) +list(APPEND RRTS_INCLUDE_DIRS PUBLIC ${PROTOBUF_INCLUDE_DIR}) +list(APPEND RRTS_LINKER_LIBS PUBLIC ${PROTOBUF_LIBRARIES}) + +# As of Ubuntu 14.04 protoc is no longer a part of libprotobuf-dev package +# and should be installed separately as in: sudo apt-get install protobuf-compiler +if(EXISTS ${PROTOBUF_PROTOC_EXECUTABLE}) + message(STATUS "Found PROTOBUF Compiler: ${PROTOBUF_PROTOC_EXECUTABLE}") +else() + message(FATAL_ERROR "Could not find PROTOBUF Compiler") +endif() + +if (PROTOBUF_FOUND) + message ("protobuf ${PROTOBUF_LIBRARIES} found") +else () + message (FATAL_ERROR "Cannot find protobuf") +endif () + +# place where to generate protobuf sources +#set(proto_gen_folder "${PROJECT_BINARY_DIR}/include/caffe/proto") +#include_directories("${PROJECT_BINARY_DIR}/include") + +set(PROTOBUF_GENERATE_CPP_APPEND_PATH TRUE) + +################################################################################################ +# Modification of standard 'protobuf_generate_cpp()' with output dir parameter +# Usage: +# caffe_protobuf_generate_cpp_py( ) +function(rrts_protobuf_generate_cpp output_dir proto_srcs proto_hdrs) + if(NOT ARGN) + message(SEND_ERROR "Error: rrts_protobuf_generate_cpp() called without any proto files") + return() + endif() + + if(PROTOBUF_GENERATE_CPP_APPEND_PATH) + # Create an include path for each file specified + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(abs_path ${abs_fil} PATH) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + else() + set(_protoc_include -I .) + endif() + + if(DEFINED PROTOBUF_IMPORT_DIRS) + foreach(dir ${PROTOBUF_IMPORT_DIRS}) + get_filename_component(abs_path ${dir} ABSOLUTE) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + endif() + + set(${proto_srcs}) + set(${proto_hdrs}) + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(fil_we ${fil} NAME_WE) + + list(APPEND ${proto_srcs} "${output_dir}/${fil_we}.pb.cc") + list(APPEND ${proto_hdrs} "${output_dir}/${fil_we}.pb.h") + + add_custom_command( + OUTPUT "${output_dir}/${fil_we}.pb.cc" + "${output_dir}/${fil_we}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${output_dir} ${_protoc_include} ${abs_fil} + DEPENDS ${abs_fil} + COMMENT "Running C++ protocol buffer compiler on ${fil}" + VERBATIM ) + endforeach() + + set_source_files_properties(${${proto_srcs}} ${${proto_hdrs}} PROPERTIES GENERATED TRUE) + set(${proto_srcs} ${${proto_srcs}} PARENT_SCOPE) + set(${proto_hdrs} ${${proto_hdrs}} PARENT_SCOPE) +endfunction() diff --git a/roborts_costmap/config/costmap_parameter_config_for_decision.prototxt b/roborts_costmap/config/costmap_parameter_config_for_decision.prototxt new file mode 100755 index 00000000..fdbbe80b --- /dev/null +++ b/roborts_costmap/config/costmap_parameter_config_for_decision.prototxt @@ -0,0 +1,40 @@ +para_basic { + is_raw_rosmessage: true + is_debug: true +} +para_costmap_interface { + global_frame: "map" + robot_base_frame: "base_link" + footprint_padding: 0.1 + transform_tolerance: 3.0 + distance_threshold: 1.0 + map_width: 10.0 + map_height: 10.0 + map_resolution: 0.05 + map_origin_x: 0.0 + map_origin_y: 0.0 + is_tracking_unknown: false + is_rolling_window: false + has_obstacle_layer: true + has_static_layer: true + inflation_file_path: "/config/inflation_layer_config.prototxt" + map_update_frequency: 5 +} +footprint { + point { + x: -0.3 + y: -0.225 + } + point { + x: -0.3 + y: 0.225 + } + point { + x: 0.3 + y: 0.225 + } + point { + x: 0.3 + y: -0.225 + } +} diff --git a/roborts_costmap/config/costmap_parameter_config_for_global_plan.prototxt b/roborts_costmap/config/costmap_parameter_config_for_global_plan.prototxt new file mode 100755 index 00000000..fdbbe80b --- /dev/null +++ b/roborts_costmap/config/costmap_parameter_config_for_global_plan.prototxt @@ -0,0 +1,40 @@ +para_basic { + is_raw_rosmessage: true + is_debug: true +} +para_costmap_interface { + global_frame: "map" + robot_base_frame: "base_link" + footprint_padding: 0.1 + transform_tolerance: 3.0 + distance_threshold: 1.0 + map_width: 10.0 + map_height: 10.0 + map_resolution: 0.05 + map_origin_x: 0.0 + map_origin_y: 0.0 + is_tracking_unknown: false + is_rolling_window: false + has_obstacle_layer: true + has_static_layer: true + inflation_file_path: "/config/inflation_layer_config.prototxt" + map_update_frequency: 5 +} +footprint { + point { + x: -0.3 + y: -0.225 + } + point { + x: -0.3 + y: 0.225 + } + point { + x: 0.3 + y: 0.225 + } + point { + x: 0.3 + y: -0.225 + } +} diff --git a/roborts_costmap/config/costmap_parameter_config_for_local_plan.prototxt b/roborts_costmap/config/costmap_parameter_config_for_local_plan.prototxt new file mode 100755 index 00000000..a9e14eab --- /dev/null +++ b/roborts_costmap/config/costmap_parameter_config_for_local_plan.prototxt @@ -0,0 +1,40 @@ +para_basic { + is_raw_rosmessage: true + is_debug: true +} +para_costmap_interface { + global_frame: "odom" + robot_base_frame: "base_link" + footprint_padding: 0.05 + transform_tolerance: 1.0 + distance_threshold: 1.0 + map_width: 5.0 + map_height: 3.0 + map_resolution: 0.04 + map_origin_x: 0.0 + map_origin_y: 0.0 + is_tracking_unknown: false + is_rolling_window: true + has_obstacle_layer: true + has_static_layer: false + inflation_file_path: "/config/inflation_layer_config_min.prototxt" + map_update_frequency: 10 +} +footprint { + point { + x: -0.3 + y: -0.225 + } + point { + x: -0.3 + y: 0.225 + } + point { + x: 0.3 + y: 0.225 + } + point { + x: 0.3 + y: -0.225 + } +} diff --git a/roborts_costmap/config/inflation_layer_config.prototxt b/roborts_costmap/config/inflation_layer_config.prototxt new file mode 100755 index 00000000..2973bc94 --- /dev/null +++ b/roborts_costmap/config/inflation_layer_config.prototxt @@ -0,0 +1,4 @@ +inflation_radius: 0.7 +cost_scaling_factor: 10 +is_raw_rosmessage: true +is_debug: true diff --git a/roborts_costmap/config/inflation_layer_config_min.prototxt b/roborts_costmap/config/inflation_layer_config_min.prototxt new file mode 100755 index 00000000..a6d16a26 --- /dev/null +++ b/roborts_costmap/config/inflation_layer_config_min.prototxt @@ -0,0 +1,4 @@ +inflation_radius: 0.7 +cost_scaling_factor: 2.5 +is_raw_rosmessage: true +is_debug: true diff --git a/roborts_costmap/config/obstacle_layer_config.prototxt b/roborts_costmap/config/obstacle_layer_config.prototxt new file mode 100755 index 00000000..fd6a04f0 --- /dev/null +++ b/roborts_costmap/config/obstacle_layer_config.prototxt @@ -0,0 +1,16 @@ +observation_keep_time: 0 +expected_update_rate: 10 +min_obstacle_height: 0.0 +max_obstacle_height: 2.0 +obstacle_range: 5 +raytrace_range: 5 +transform_tolerance: 0.3 +topic_string: "scan" +sensor_frame: "base_laser_link" +inf_is_valid: false +clearing: true +footprint_clearing_enabled: false +marking: true +is_debug: true + + diff --git a/roborts_costmap/config/static_layer_config.prototxt b/roborts_costmap/config/static_layer_config.prototxt new file mode 100755 index 00000000..d659136a --- /dev/null +++ b/roborts_costmap/config/static_layer_config.prototxt @@ -0,0 +1,10 @@ +first_map_only: false +subscribe_to_updates: false +track_unknown_space: true +use_maximum: true +unknown_cost_value: -1 +lethal_threshold: 100 +trinary_map: true +topic_name: "map" +is_raw_rosmessage: true +is_debug: true \ No newline at end of file diff --git a/roborts_costmap/include/costmap/costmap_2d.h b/roborts_costmap/include/costmap/costmap_2d.h new file mode 100755 index 00000000..9f3f10a1 --- /dev/null +++ b/roborts_costmap/include/costmap/costmap_2d.h @@ -0,0 +1,482 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_COSTMAP_2D_H +#define ROBORTS_COSTMAP_COSTMAP_2D_H + +#include +#include +#include +#include + +namespace roborts_costmap{ + +/** + * @brief A struct representing a map point coordinate + */ +struct MapLocation { + unsigned int x; + unsigned int y; +}; + +/** + * @class Costmap2D + * @brief Map provides a mapping between the world and map cost + */ +class Costmap2D { + public: + /** + * @brief Constructor for a costmap + * @param cells_size_x The x size of the map in cells + * @param cells_size_y The y size of the map in cells + * @param resolution The resolution of the map in meters/cell + * @param origin_x The x origin of the map in meters + * @param origin_y The y origin of the map in meters + * @param default_value Default Value + */ + Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, + double origin_x, double origin_y, unsigned char default_value = 0); + + /** + * @brief Copy constructor for a costmap, creates a copy efficiently + * @param map The costmap to copy + */ + Costmap2D(const Costmap2D &map); + + /** + * @brief Overloaded assignment operator + * @param map The costmap to copy + * @return A reference to the map after the copy has finished + */ + Costmap2D &operator=(const Costmap2D &map); + + /** + * @brief Turn this costmap into a copy of a window of a costmap passed in + * @param map The costmap to copy + * @param win_origin_x The x origin (lower left corner) for the window to copy, in meters + * @param win_origin_y The y origin (lower left corner) for the window to copy, in meters + * @param win_size_x The x size of the window, in meters + * @param win_size_y The y size of the window, in meters + */ + bool CopyCostMapWindow(const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, + double win_size_y); + + /** + * @brief Default constructor + */ + Costmap2D(); + + /** + * @brief Destructor + */ + virtual ~Costmap2D(); + + /** + * @brief Get the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The cost of the cell + */ + unsigned char GetCost(unsigned int mx, unsigned int my) const; + + /** + * @brief Set the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @param cost The cost to set the cell to + */ + void SetCost(unsigned int mx, unsigned int my, unsigned char cost); + + /** + * @brief Convert from map coordinates to world coordinates + * @param mx The x map coordinate + * @param my The y map coordinate + * @param wx Will be set to the associated world x coordinate + * @param wy Will be set to the associated world y coordinate + */ + void Map2World(unsigned int mx, unsigned int my, double &wx, double &wy) const; + + /** + * @brief Convert from world coordinates to map coordinates + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool World2Map(double wx, double wy, unsigned int &mx, unsigned int &my) const; + + /** + * @brief Convert from world coordinates to map coordinates without checking for legal bounds + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @note The returned map coordinates are not guaranteed to lie within the map. + */ + void World2MapNoBoundary(double wx, double wy, int &mx, int &my) const; + + /** + * @brief Convert from world coordinates to map coordinates, constraining results to legal bounds. + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @note The returned map coordinates are guaranteed to lie in the map. + */ + void World2MapWithBoundary(double wx, double wy, int &mx, int &my) const; + + /** + * @brief Given two map coordinates... compute the associated index + * @param mx The x coordinate + * @param my The y coordinate + * @return The associated index + */ + inline unsigned int GetIndex(unsigned int mx, unsigned int my) const { + return my * size_x_ + mx; + } + + /** + * @brief Given an index, compute the associated map coordinates + * @param index The index + * @param mx Will be set to the x coordinate + * @param my Will be set to the y coordinate + */ + inline void Index2Cells(unsigned int index, unsigned int &mx, unsigned int &my) const { + my = index / size_x_; + mx = index - (my * size_x_); + } + + /** + * @brief Will return a pointer to the underlying unsigned char array used as the costmap + * @return A pointer to the underlying unsigned char array storing cost values + */ + unsigned char *GetCharMap() const; + + /** + * @brief Accessor for the x size of the costmap in cells + * @return The x size of the costmap in cells + */ + unsigned int GetSizeXCell() const; + + /** + * @brief Accessor for the y size of the costmap in cells + * @return The y size of the costmap in cells + */ + unsigned int GetSizeYCell() const; + + /** + * @brief Accessor for the x size of the costmap in meters + * @return The x size of the costmap (returns the centerpoint of the last legal cell in the map) + */ + double GetSizeXWorld() const; + + /** + * @brief Accessor for the y size of the costmap in meters + * @return The y size of the costmap (returns the centerpoint of the last legal cell in the map) + */ + double GetSizeYWorld() const; + + /** + * @brief Accessor for the x origin of the costmap + * @return The x origin of the costmap + */ + double GetOriginX() const; + + /** + * @brief Accessor for the y origin of the costmap + * @return The y origin of the costmap + */ + double GetOriginY() const; + + /** + * @brief Accessor for the resolution of the costmap + * @return The resolution of the costmap + */ + double GetResolution() const; + /** + * @brief Set the default map of the costmap + * @param c the cost value to set + */ + void SetDefaultValue(unsigned char c) { + default_value_ = c; + } + /** + * @brief Get the default value of the costmap + * @return the default value of the map + */ + unsigned char GetDefaultValue() const { + return default_value_; + } + + /** + * @brief Sets the cost of a convex polygon to a desired value + * @param polygon The polygon to perform the operation on + * @param cost_value The value to set costs to + * @return True if the polygon was filled, false if it could not be filled + */ + bool SetConvexRegionCost(const std::vector &polygon_edge_world, unsigned char value); + + /**polygonOutlineCells + * @brief Get the map cells that make up the outline of a polygon + * @param convex_region_cells The polygon in map coordinates to rasterize + * @param convex_edge_cells Will be set to the cells contained in the outline of the polygon + */ + + void GetConvexEdge(const std::vector &convex_region_cells, std::vector &convex_edge_cells); + + /**convexFillCells + * @brief Get the map cells that fill a convex polygon + * @param convex_edge_cells The polygon in map coordinates to rasterize + * @param convex_region_cells Will be set to the cells that fill the polygon + */ + void FillConvexCells(const std::vector &convex_edge_cells, + std::vector &convex_region_cells); + + /** + * @brief Move the origin of the costmap to a new location.... keeping data when it can + * @param new_origin_x The x coordinate of the new origin + * @param new_origin_y The y coordinate of the new origin + */ + virtual void UpdateOrigin(double new_origin_x, double new_origin_y); + + /** + * @brief Save the costmap out to a pgm file + * @param file_name The name of the file to save + */ + bool SaveMap(std::string map_name); + + /** + * @brief Reset the size of the map + * @param size_x The new size x to set + * @param size_y The new size y to set + * @param resolution The new resolution to set + * @param origin_x The new origin x to set + * @param origin_y The new origin y to set + */ + void ResizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, + double origin_y); + + void ResetPartMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn); + + /** + * @brief Given distance in the world, convert it to cells + * @param world_dist The world distance + * @return The equivalent cell distance + */ + unsigned int World2Cell(double world_dist); + + typedef std::recursive_mutex mutex_t; + mutex_t *GetMutex() { + return access_; + } + + protected: + +/** + * @brief Copy a region of a source map into a destination map + * @tparam maptype + * @param src_map The source map + * @param dst_map The destination map + * @param src_map_size_x The size X of the src + * @param dst_map_size_x The size X of the dst + * @param src_map_lower_left_x_start_to_copy The start point to copy in src x + * @param src_map_lower_left_y_start_to_copy The start point to copy in src y + * @param dst_map_lower_left_x_start_to_copy The start point to copy in dst x + * @param dst_map_lower_left_y_start_to_copy The start point to copy in dst y + * @param copy_region_size_x The copy window size X + * @param copy_region_size_y The copy window size Y + */ + template + void CopyMapRegion(maptype *src_map, + maptype *dst_map, \ + + unsigned int src_map_size_x, + unsigned int dst_map_size_x, \ + + unsigned int src_map_lower_left_x_start_to_copy, + unsigned int src_map_lower_left_y_start_to_copy, \ + + unsigned int dst_map_lower_left_x_start_to_copy, + unsigned int dst_map_lower_left_y_start_to_copy, \ + + unsigned int copy_region_size_x, + unsigned int copy_region_size_y) { + maptype *src_copy_ptr = + src_map + (src_map_lower_left_x_start_to_copy + src_map_lower_left_y_start_to_copy * src_map_size_x); + maptype *dst_copy_ptr = + dst_map + (dst_map_lower_left_x_start_to_copy + dst_map_lower_left_y_start_to_copy * dst_map_size_x); + for (unsigned i = 0; i < copy_region_size_y; ++i) { + memcpy(dst_copy_ptr, src_copy_ptr, copy_region_size_x * sizeof(maptype)); + dst_copy_ptr += dst_map_size_x; + src_copy_ptr += src_map_size_x; + } + } + + /** + * @brief Deletes the costmap, static_map, and markers data structures + */ + virtual void DeleteMaps(); + + /** + * @brief Reset the costmap and static_map to be unknown space + */ + virtual void ResetMaps(); + + /** + * @brief Initialize the costmap, static_map, and markers data structures + * @param size_x The x size to use for map initialization + * @param size_y The y size to use for map initialization + */ + virtual void InitMaps(unsigned int size_x, unsigned int size_y); + + /** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param max_length The maximum desired length of the segment.allowing to not go all the way to the endpoint + */ + template + inline void RaytraceLine(ActionType at, unsigned int x0, unsigned int y0, \ + unsigned int x1, unsigned int y1, \ + unsigned int max_length = UINT_MAX) { + int dx = x1 - x0; + int dy = y1 - y0; + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + int offset_dx = Sign(dx); + int offset_dy = Sign(dy) * size_x_; + unsigned int offset = y0 * size_x_ + x0; + double dist = hypot(dx, dy); + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + // if x is dominant + if (abs_dx >= abs_dy) { + int error_y = abs_dx / 2; + Bresenham(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int) (scale * abs_dx)); + return; + } + // otherwise y is dominant + int error_x = abs_dy / 2; + Bresenham(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int) (scale * abs_dy)); + } + + private: + /** + * @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step + */ + template + inline void Bresenham(ActionType at, unsigned int abs_da, unsigned int abs_db, \ + int error_b, int offset_a, \ + int offset_b, unsigned int offset, unsigned int max_length) { + unsigned int end = std::min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) { + at(offset); + offset += offset_a; + error_b += abs_db; + if ((unsigned int) error_b >= abs_da) { + offset += offset_b; + error_b -= abs_da; + } + } + at(offset); + } + + inline int Sign(int x) { + return x > 0 ? 1 : -1; + } + + mutex_t *access_; + protected: + unsigned int size_x_; + unsigned int size_y_; + double resolution_; + double origin_x_; + double origin_y_; + unsigned char *costmap_; + unsigned char default_value_; + + class MarkCell { + public: + MarkCell(unsigned char *costmap, unsigned char value) : + costmap_(costmap), value_(value) {} + inline void operator()(unsigned int offset) { + costmap_[offset] = value_; + } + private: + unsigned char *costmap_; + unsigned char value_; + }; + + class PolygonOutlineCells { + public: + PolygonOutlineCells(const Costmap2D &costmap, const unsigned char *char_map, std::vector &cells) : + costmap_(costmap), char_map_(char_map), cells_(cells) { + } + + // just push the relevant cells back onto the list + inline void operator()(unsigned int offset) { + MapLocation loc; + costmap_.Index2Cells(offset, loc.x, loc.y); + cells_.push_back(loc); + } + + private: + const Costmap2D &costmap_; + const unsigned char *char_map_; + std::vector &cells_; + }; +}; +} // namespace roborts_costmap + +#endif //ROBORTS_COSTMAP_COSTMAP_2D_H diff --git a/roborts_costmap/include/costmap/costmap_interface.h b/roborts_costmap/include/costmap/costmap_interface.h new file mode 100755 index 00000000..f34291f0 --- /dev/null +++ b/roborts_costmap/include/costmap/costmap_interface.h @@ -0,0 +1,260 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_COSTMAP_INTERFACE_H +#define ROBORTS_COSTMAP_COSTMAP_INTERFACE_H + +#include +#include +#include + +#include +#include +#include "map_common.h" +#include "footprint.h" +#include "layer.h" +#include "layered_costmap.h" +#include "costmap_layer.h" +#include "static_layer.h" +#include "obstacle_layer.h" +#include "inflation_layer.h" + +namespace roborts_costmap { + +/** + * @brief Robot pose with time stamp and global frame id. + */ +typedef struct { + ros::Time time; + std::string frame_id; + Eigen::Vector3f position; + Eigen::Matrix3f rotation; +} RobotPose; + +/** + * @brief class Costmap Interface for users. + */ +class CostmapInterface { + public: + /** + * @brief Constructor + * @param map_name The costmap name + * @param tf The tf listener + * @param map_update_frequency The frequency to update costmap + */ + CostmapInterface(std::string map_name, tf::TransformListener& tf, std::string config_file); + ~CostmapInterface(); + /** + * @brief Start the costmap processing. + */ + void Start(); + /** + * @breif Stop the costmap. + */ + void Stop(); + /** + * @breif If costmap is stoped or paused, it can be resumed. + */ + void Resume(); + /** + * @breif The core function updating the costmap with all layers. + */ + void UpdateMap(); + /** + * @brief Function to pause the costmap disabling the update. + */ + void Pause(); + /** + * @breif Reset costmap with every layer reset. + */ + void ResetLayers(); + /** + * @brief Tell whether the costmap is updated. + * @return True if every layer is current. + */ + bool IsCurrent() { + return layered_costmap_->IsCurrent(); + } + /** + * @brief Get the robot pose in global frame. + * @param global_pose + * @return True if got successfully. + */ + bool GetRobotPose(tf::Stamped& global_pose) const; + /** + * @brief Get the costmap. + * @return The class containing costmap data. + */ + Costmap2D* GetCostMap() const { + return layered_costmap_->GetCostMap(); + } + /** + * @brief Get robot pose with time stamped. + * @param global_pose + * @return True if success. + */ + bool GetRobotPose(geometry_msgs::PoseStamped & global_pose) const; + /** + * @brief Get the global map frame. + * @return The global map frame name. + */ + std::string GetGlobalFrameID() { + return global_frame_; + } + /** + * @brief Get the base frame. + * @return The base frame name. + */ + std::string GetBaseFrameID() { + return robot_base_frame_; + } + /** + * @brief Get the layered costmap + * @return The class including layers + */ + CostmapLayers* GetLayeredCostmap() { + return layered_costmap_; + } + /** + * @brief Get the footprint polygon which are already padded. + * @return The footprint polygon. + */ + geometry_msgs::Polygon GetRobotFootprintPolygon() { + return ToPolygon(padded_footprint_); + } + /** + * @brief Get the footprint points which are already padded. + * @return The vector of footprint point. + */ + std::vector GetRobotFootprint() { + return padded_footprint_; + } + /** + * @brief Get the footprint which are not padded. + * @return The vector of footprint point. + */ + std::vector GetUnpaddedRobotFootprint() { + return unpadded_footprint_; + } + /** + * @brief Get the oriented footprint in global map. + * @param oriented_footprint + */ + void GetOrientedFootprint(std::vector& oriented_footprint) const; + /** + * @brief Set up the robot footprint. + * @param points Input vector of points to setup the robot footprint. + */ + void SetUnpaddedRobotFootprint(const std::vector& points); + /** + * @brief Set up the robot footprint. + * @param footprint Input po"CostmapInterface"lygon to setup the robot footprint. + */ + void SetUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint); + + /** + * @brief Output the original footprint. + * @param footprint Vector of Eigen::Vector3f. + */ + void GetFootprint(std::vector & footprint); + /** + * @brief Get the oriented footprint. + * @param footprint Vector of Eigen::Vector3f + */ + void GetOrientedFootprint(std::vector & footprint); + /** + * @brief Get robot pose in self-defined format. + * @param pose Pose defined by RM team. + * @return True if get successfully + */ + bool GetRobotPose(RobotPose & pose); + /** + * @brief Get the costmap value array. + * @return The pointer to the costmap value array. + */ + unsigned char* GetCharMap() const; + /** + * @brief Get the stamped pose message. + * @param pose_msg + * @return PoseStamped message. + */ + geometry_msgs::PoseStamped Pose2GlobalFrame(const geometry_msgs::PoseStamped& pose_msg); + void ClearCostMap(); + void ClearLayer(CostmapLayer* costmap_layer_ptr, double pose_x, double pose_y); + protected: + void LoadParameter(); + std::vector footprint_points_; + CostmapLayers* layered_costmap_; + std::string name_, config_file_, config_file_inflation_; + tf::TransformListener& tf_; + std::string global_frame_, robot_base_frame_; + double transform_tolerance_, dist_behind_robot_threshold_to_care_obstacles_; + nav_msgs::OccupancyGrid grid_; + char* cost_translation_table_ = new char[256]; + + ros::Publisher costmap_pub_; + + private: + void DetectMovement(const ros::TimerEvent &event); + void MapUpdateLoop(double frequency); + std::vector unpadded_footprint_, padded_footprint_; + float footprint_padding_; + bool map_update_thread_shutdown_, stop_updates_, initialized_, stopped_, robot_stopped_, got_footprint_, is_debug_, \ + is_track_unknown_, is_rolling_window_, has_static_layer_, has_obstacle_layer_; + double map_update_frequency_, map_width_, map_height_, map_origin_x_, map_origin_y_, map_resolution_; + std::thread* map_update_thread_; + ros::Timer timer_; + ros::Time last_publish_; + tf::Stamped old_pose_; +}; + +} //namespace roborts_costmap +#endif // ROBORTS_COSTMAP_COSTMAP_INTERFACE_H diff --git a/roborts_costmap/include/costmap/costmap_layer.h b/roborts_costmap/include/costmap/costmap_layer.h new file mode 100755 index 00000000..ae8ae725 --- /dev/null +++ b/roborts_costmap/include/costmap/costmap_layer.h @@ -0,0 +1,159 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef ROBORTS_COSTMAP_COSTMAP_LAYER_H +#define ROBORTS_COSTMAP_COSTMAP_LAYER_H + +#include +#include "layer.h" +#include "layered_costmap.h" +#include "costmap_2d.h" + +namespace roborts_costmap { + +class CostmapLayer : public Layer, public Costmap2D { + public: + CostmapLayer() : has_extra_bounds_(false), + extra_min_x_(1e6), extra_max_x_(-1e6), + extra_min_y_(1e6), extra_max_y_(-1e6) {} + + virtual void MatchSize(); + + /** + * If an external source changes values in the costmap, + * it should call this method with the area that it changed + * to ensure that the costmap includes this region as well. + * @param mx0 Minimum x value of the bounding box + * @param my0 Minimum y value of the bounding box + * @param mx1 Maximum x value of the bounding box + * @param my1 Maximum y value of the bounding box + */ + void AddExtraBounds(double mx0, double my0, double mx1, double my1); + + protected: + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * All means every value from this layer + * is written into the master grid. + */ + void UpdateOverwriteByAll(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j); + + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Valid means every valid value from this layer + * is written into the master grid (does not copy NO_INFORMATION) + */ + void UpdateOverwriteByValid(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j); + + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Sets the new value to the maximum of the master_grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is overwritten. If the layer's value is NO_INFORMATION, + * the master value does not change. + */ + void UpdateOverwriteByMax(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j); + + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Sets the new value to the sum of the master grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is overwritten with the layer's value. If the layer's value + * is NO_INFORMATION, then the master value does not change. + * + * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE, + * the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1). + */ + void UpdateOverwriteByAdd(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j); + + /** + * Updates the bounding box specified in the parameters to include + * the location (x,y) + * + * @param x x-coordinate to include + * @param y y-coordinate to include + * @param min_x bounding box + * @param min_y bounding box + * @param max_x bounding box + * @param max_y bounding box + */ + void Touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y); + + /* + * Updates the bounding box specified in the parameters + * to include the bounding box from the addExtraBounds + * call. If addExtraBounds was not called, the method will do nothing. + * + * Should be called at the beginning of the updateBounds method + * + * @param min_x bounding box (input and output) + * @param min_y bounding box (input and output) + * @param max_x bounding box (input and output) + * @param max_y bounding box (input and output) + */ + void UseExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y); + bool has_extra_bounds_; + + private: + double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; +}; + +} // namespace roborts_costmap +#endif // ROBORTS_COSTMAP_COSTMAP_LAYER_H diff --git a/roborts_costmap/include/costmap/costmap_math.h b/roborts_costmap/include/costmap/costmap_math.h new file mode 100755 index 00000000..698b8bc8 --- /dev/null +++ b/roborts_costmap/include/costmap/costmap_math.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include +#include +#include +#include +namespace roborts_costmap { +/** @brief Return -1 if x < 0, +1 otherwise. */ +inline double sign(double x) { + return x < 0.0 ? -1.0 : 1.0; +} + +/** @brief Same as sign(x) but returns 0 if x is 0. */ +inline double sign0(double x) { + return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); +} + +inline double distance(double x0, double y0, double x1, double y1) { + return hypot(x1 - x0, y1 - y0); +} + +double Distance2Line(double pX, double pY, double x0, double y0, double x1, double y1); + +bool Intersect(std::vector &polygon, float testx, float testy); + +bool Intersect(std::vector &polygon1, std::vector &polygon2); +}//namespace roborts_costmap + diff --git a/roborts_costmap/include/costmap/footprint.h b/roborts_costmap/include/costmap/footprint.h new file mode 100755 index 00000000..d468c95b --- /dev/null +++ b/roborts_costmap/include/costmap/footprint.h @@ -0,0 +1,159 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_FOOTPRINT_H +#define ROBORTS_COSTMAP_FOOTPRINT_H + +#include +#include +#include +#include +#include + +namespace roborts_costmap { + +/** + * @brief Calculate the extreme distances for the footprint + * + * @param footprint The footprint to examine + * @param min_dist Output parameter of the minimum distance + * @param max_dist Output parameter of the maximum distance + */ +void CalculateMinAndMaxDistances(const std::vector& footprint, + double& min_dist, double& max_dist); + +/** + * @brief Convert Point32 to Point + */ +geometry_msgs::Point ToPoint(geometry_msgs::Point32 pt); + +/** + * @brief Convert Point to Point32 + */ +geometry_msgs::Point32 ToPoint32(geometry_msgs::Point pt); + +/** + * @brief Convert vector of Points to Polygon msg + */ +geometry_msgs::Polygon ToPolygon(std::vector pts); + +/** + * @brief Convert Polygon msg to vector of Points. + */ +std::vector ToPointVector(geometry_msgs::Polygon polygon); + +/** + * @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points) + * @param x The x position of the robot + * @param y The y position of the robot + * @param theta The orientation of the robot + * @param footprint_spec Basic shape of the footprint + * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot +*/ +void TransformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& oriented_footprint); + +/** + * @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped) + * @param x The x position of the robot + * @param y The y position of the robot + * @param theta The orientation of the robot + * @param footprint_spec Basic shape of the footprint + * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot +*/ +void TransformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + geometry_msgs::PolygonStamped & oriented_footprint); + +/** + * @brief Adds the specified amount of padding to the footprint (in place) + */ +void PadFootprint(std::vector& footprint, double padding); + +/** + * @brief Create a circular footprint from a given radius + */ +std::vector MakeFootprintFromRadius(double radius); + +/** + * @brief Make the footprint from the given string. + * + * Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...] + * + */ +bool MakeFootprintFromString(const std::string& footprint_string, std::vector& footprint); + +/** + * @brief Read the ros-params "footprint" and/or "robot_radius" from + * the given NodeHandle using searchParam() to go up the tree. + */ +std::vector MakeFootprintFromParams(ros::NodeHandle& nh); + +/** + * @brief Create the footprint from the given XmlRpcValue. + * + * @param footprint_xmlrpc should be an array of arrays, where the + * top-level array should have 3 or more elements, and the + * sub-arrays should all have exactly 2 elements (x and y + * coordinates). + * + * @param full_param_name this is the full name of the rosparam from + * which the footprint_xmlrpc value came. It is used only for + * reporting errors. */ +std::vector MakeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, + const std::string& full_param_name); + +/** @brief Write the current unpadded_footprint_ to the "footprint" + * parameter of the given NodeHandle so that dynamic_reconfigure + * will see the new value. */ +void WriteFootprintToParam(ros::NodeHandle& nh, const std::vector& footprint); + +} //namespace roborts_costmap +#endif // ROBORTS_COSTMAP_FOOTPRINT_H diff --git a/roborts_costmap/include/costmap/inflation_layer.h b/roborts_costmap/include/costmap/inflation_layer.h new file mode 100755 index 00000000..0d74fdf3 --- /dev/null +++ b/roborts_costmap/include/costmap/inflation_layer.h @@ -0,0 +1,192 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_INFLATION_LAYER_H +#define ROBORTS_COSTMAP_INFLATION_LAYER_H + +#include +#include "map_common.h" +#include "layer.h" +#include "layered_costmap.h" + +namespace roborts_costmap { + +/** + * @class CellData + * @brief Storage for cell information used during obstacle inflation + */ +class CellData { + public: + /** + * @brief Constructor for a CellData objects + * @param i The index of the cell in the cost map + * @param x The x coordinate of the cell in the cost map + * @param y The y coordinate of the cell in the cost map + * @param sx The x coordinate of the closest obstacle cell in the costmap + * @param sy The y coordinate of the closest obstacle cell in the costmap + * @return + */ + CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : + index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) { + } + unsigned int index_; + unsigned int x_, y_; + unsigned int src_x_, src_y_; +}; + +class InflationLayer : public Layer { + public: + InflationLayer(); + + virtual ~InflationLayer() { + DeleteKernels(); + } + + virtual void OnInitialize(); + virtual void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, + double *max_x, double *max_y); + virtual void UpdateCosts(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j); + virtual bool IsDiscretized() { + return true; + } + virtual void MatchSize(); + + virtual void Reset() { OnInitialize(); } + + /** @brief Given a distance, compute a cost. + * @param distance The distance from an obstacle in cells + * @return A cost value for the distance */ + inline unsigned char ComputeCost(unsigned distance_cell) const { + unsigned char cost = 0; + if (distance_cell == 0) + cost = LETHAL_OBSTACLE; + else if (distance_cell * resolution_ <= inscribed_radius_) + cost = INSCRIBED_INFLATED_OBSTACLE; + else { + // make sure cost falls off by Euclidean distance + double euclidean_distance = distance_cell * resolution_; + double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); + cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); + } + return cost; + } + + /** + * @brief Change the values of the inflation radius parameters + * @param inflation_radius The new inflation radius + * @param cost_scaling_factor The new weight + */ + void SetInflationParameters(double inflation_radius, double cost_scaling_factor); + + protected: + virtual void OnFootprintChanged(); + std::recursive_mutex *inflation_access_; + + private: + /** + * @brief Lookup pre-computed distances + * @param mx The x coordinate of the current cell + * @param my The y coordinate of the current cell + * @param src_x The x coordinate of the source cell + * @param src_y The y coordinate of the source cell + * @return + */ + inline double DistanceLookup(int mx, int my, int src_x, int src_y) { + unsigned int dx = abs(mx - src_x); + unsigned int dy = abs(my - src_y); + return cached_distances_[dx][dy]; + } + + /** + * @brief Lookup pre-computed costs + * @param mx The x coordinate of the current cell + * @param my The y coordinate of the current cell + * @param src_x The x coordinate of the source cell + * @param src_y The y coordinate of the source cell + * @return + */ + inline unsigned char CostLookup(int mx, int my, int src_x, int src_y) { + unsigned int dx = abs(mx - src_x); + unsigned int dy = abs(my - src_y); + return cached_costs_[dx][dy]; + } + + void ComputeCaches(); + void DeleteKernels(); + void InflateArea(int min_i, int min_j, int max_i, int max_j, unsigned char *master_grid); + + unsigned int CellDistance(double world_dist) { + return layered_costmap_->GetCostMap()->World2Cell(world_dist); + } + + inline void Enqueue(unsigned int index, unsigned int mx, unsigned int my, + unsigned int src_x, unsigned int src_y); + + double inflation_radius_, inscribed_radius_, weight_; + bool inflate_unknown_; + unsigned int cell_inflation_radius_; + unsigned int cached_cell_inflation_radius_; + std::map > inflation_cells_; + + double resolution_; + + bool* seen_; + int seen_size_; + + unsigned char** cached_costs_; + double** cached_distances_; + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + bool need_reinflation_; +}; + +} //namespace roborts_costmap +#endif //ROBORTS_COSTMAP_INFLATION_LAYER_H \ No newline at end of file diff --git a/roborts_costmap/include/costmap/layer.h b/roborts_costmap/include/costmap/layer.h new file mode 100755 index 00000000..9b5242a2 --- /dev/null +++ b/roborts_costmap/include/costmap/layer.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_LAYER_H +#define ROBORTS_COSTMAP_LAYER_H + +#include +#include +#include + +#include "costmap_2d.h" +#include "layered_costmap.h" + +namespace roborts_costmap { + +class CostmapLayers; + +class Layer { + public: +/** + * @brief constructor + */ + Layer(); +/** + * @brief initialize + * @param parent the layered costmap, ie master grid + * @param name this layer name + * @param tf a tf listener providing transforms + */ + void Initialize(CostmapLayers *parent, std::string name, tf::TransformListener *tf); + +/** + * @brief This is called by the LayeredCostmap to poll this plugin as to how + * much of the costmap it needs to update. Each layer can increase + * the size of this bounds. * + * @param robot_x + * @param robot_y + * @param robot_yaw these point the pose of the robot in global frame + * @param min_x + * @param min_y + * @param max_x + * @param max_y these declare the updating boundary + */ + virtual void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, + double *max_x, double *max_y) {} + +/** + * @brief Actually update the underlying costmap, only within the bounds + * calculated during UpdateBounds(). * + * @param master_grid the master map + * @param min_i + * @param min_j + * @param max_i + * @param max_j the update boundary + */ + virtual void UpdateCosts(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) {} + + /** + * @brief Stop. + */ + virtual void Deactivate() {} + + /** + * @brief Restart, if they've been stopped. + */ + virtual void Activate() {} + + /** + * @brief Reset the layer + */ + virtual void Reset() {} + + virtual ~Layer() {} + + /** + * @brief Check to make sure all the data in the layer is update. + * @return Whether the data in the layer is up to date. + */ + bool IsCurrent() const { + return is_current_; + } + + /** + * @brief Implement this to make this layer match the size of the parent costmap. + */ + virtual void MatchSize() {} + + std::string GetName() const { + return name_; + } + + /** + * @brief Convenience function for layered_costmap_->GetFootprint(). + */ + const std::vector &GetFootprint() const; + + virtual void OnFootprintChanged() {} + + protected: + /** @brief This is called at the end of initialize(). Override to + * implement subclass-specific initialization. + * tf_, name_, and layered_costmap_ will all be set already when this is called. */ + virtual void OnInitialize() {} + + CostmapLayers *layered_costmap_; + bool is_current_, is_enabled_, is_debug_; + std::string name_; + tf::TransformListener *tf_; + + private: + std::vector footprint_spec_; +}; + +} //namespace roborts_costmap +#endif // ROBORTS_COSTMAP_LAYER_H diff --git a/roborts_costmap/include/costmap/layered_costmap.h b/roborts_costmap/include/costmap/layered_costmap.h new file mode 100755 index 00000000..86b04c9d --- /dev/null +++ b/roborts_costmap/include/costmap/layered_costmap.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_COSTMAPLAYERS_H +#define ROBORTS_COSTMAP_COSTMAPLAYERS_H + +#include +#include "map_common.h" +#include "layer.h" +#include "costmap_2d.h" +#include "footprint.h" + +namespace roborts_costmap { + +class Layer; + +class CostmapLayers { + public: + CostmapLayers(std::string global_frame, bool rolling_window, bool track_unknown); + ~CostmapLayers(); + void UpdateMap(double robot_x, double robot_y, double robot_yaw); + + const std::vector& GetFootprint() { + return footprint_; + } + + std::string GetGlobalFrameID() const { + return global_frame_id_; + } + + bool IsRollingWindow() const { + return is_rolling_window_; + } + + bool IsSizeLocked() const { + return is_size_locked_; + } + + bool IsTrackingUnknown() const { + return costmap_.GetDefaultValue() == NO_INFORMATION; + } + + void ResizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, + bool size_locked = false); + + Costmap2D* GetCostMap() { + return &costmap_; + } + + void GetUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy) { + minx = minx_; + miny = miny_; + maxx = maxx_; + maxy = maxy_; + } + + bool IsCurrent(); + + bool IsRolling() { + return is_rolling_window_; + } + + std::vector* GetPlugins() { + return &plugins_; + } + + void AddPlugin(Layer* plugin) { + plugins_.push_back(plugin); + } + + bool IsSizeLocked() + { + return is_size_locked_; + } + + void GetBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn) + { + *x0 = bx0_; + *xn = bxn_; + *y0 = by0_; + *yn = byn_; + } + + bool IsInitialized() + { + return is_initialized_; + } + + /** @brief Updates the stored footprint, updates the circumscribed + * and inscribed radii, and calls onFootprintChanged() in all + * layers. */ + void SetFootprint(const std::vector& footprint_spec); + + /** @brief The radius of a circle centered at the origin of the + * robot which just surrounds all points on the robot's + * footprint. + * + * This is updated by setFootprint(). */ + double GetCircumscribedRadius() { return circumscribed_radius_; } + + /** @brief The radius of a circle centered at the origin of the + * robot which is just within all points and edges of the robot's + * footprint. + * + * This is updated by setFootprint(). */ + double GetInscribedRadius() { return inscribed_radius_; } + /** + * @brief Set the inflation layer prototxt path for the inflaiton layer to read in + * @param path The file path. + */ + void SetFilePath(const std::string &path) { + file_path_ = path; + } + + std::string GetFilePath() const { + return file_path_; + } + + private: + std::string global_frame_id_, file_path_; + std::vector footprint_; + Costmap2D costmap_; + bool is_rolling_window_, is_size_locked_, is_initialized_, is_current_; + double minx_, miny_, maxx_, maxy_, circumscribed_radius_, inscribed_radius_; + unsigned int bx0_, bxn_, by0_, byn_; + std::vector plugins_; +}; + +} //namespace roborts_costmap +#endif //ROBORTS_COSTMAP_COSTMAPLAYERS_H diff --git a/roborts_costmap/include/costmap/map_common.h b/roborts_costmap/include/costmap/map_common.h new file mode 100755 index 00000000..272432af --- /dev/null +++ b/roborts_costmap/include/costmap/map_common.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#ifndef ROBORTS_COSTMAP_MAP_COMMON_H +#define ROBORTS_COSTMAP_MAP_COMMON_H + +#include +#include +#include +#include +#include +#include + +#include "io/io.h" + +namespace roborts_costmap { +static const unsigned char NO_INFORMATION = 255; +static const unsigned char LETHAL_OBSTACLE = 254; +static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; +static const unsigned char FREE_SPACE = 0; +} //namespace roborts_costmap + +#endif //ROBORTS_COSTMAP_MAP_COMMON_H diff --git a/roborts_costmap/include/costmap/observation.h b/roborts_costmap/include/costmap/observation.h new file mode 100755 index 00000000..a71029ab --- /dev/null +++ b/roborts_costmap/include/costmap/observation.h @@ -0,0 +1,120 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_OBSERVATION_H +#define ROBORTS_COSTMAP_OBSERVATION_H +#include +#include +#include + +namespace roborts_costmap { + +/** + * @brief Stores an observation in terms of a point cloud and the origin of the source + * + */ +class Observation +{ + public: + /** + * @brief Creates an empty observation + */ + Observation() : + cloud_(new pcl::PointCloud()), obstacle_range_(0.0), raytrace_range_(0.0) + { + } + + virtual ~Observation() + { + delete cloud_; + } + + /** + * @brief Creates an observation from an origin point and a point cloud + * @param origin The origin point of the observation + * @param cloud The point cloud of the observation + * @param obstacle_range The range out to which an observation should be able to insert obstacles + * @param raytrace_range The range out to which an observation should be able to clear via raytracing + */ + Observation(geometry_msgs::Point& origin, pcl::PointCloud cloud, + double obstacle_range, double raytrace_range) : + origin_(origin), cloud_(new pcl::PointCloud(cloud)), + obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) + { + } + + /** + * @brief Copy constructor + * @param obs The observation to copy + */ + Observation(const Observation& obs) : + origin_(obs.origin_), cloud_(new pcl::PointCloud(*(obs.cloud_))), + obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_) + { + } + + /** + * @brief Creates an observation from a point cloud + * @param cloud The point cloud of the observation + * @param obstacle_range The range out to which an observation should be able to insert obstacles + */ + Observation(pcl::PointCloud cloud, double obstacle_range) : + cloud_(new pcl::PointCloud(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0) + { + } + + geometry_msgs::Point origin_; + pcl::PointCloud* cloud_; + double obstacle_range_, raytrace_range_; +}; + +}// namespace roborts_costmap +#endif // ROBORTS_COSTMAP_OBSERVATION_H \ No newline at end of file diff --git a/roborts_costmap/include/costmap/observation_buffer.h b/roborts_costmap/include/costmap/observation_buffer.h new file mode 100755 index 00000000..acc4c926 --- /dev/null +++ b/roborts_costmap/include/costmap/observation_buffer.h @@ -0,0 +1,171 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_OBSERVATION_BUFFER_H +#define ROBORTS_COSTMAP_OBSERVATION_BUFFER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "observation.h" + +namespace roborts_costmap { +/** + * @class ObservationBuffer + * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them + */ +class ObservationBuffer { + public: + /** + * @brief Constructs an observation buffer + * @param topic_name The topic of the observations, used as an identifier for error and warning messages + * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest + * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit + * @param min_obstacle_height The minimum height of a hitpoint to be considered legal + * @param max_obstacle_height The minimum height of a hitpoint to be considered legal + * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles + * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space + * @param tf A reference to a TransformListener + * @param global_frame The frame to transform PointClouds into + * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages + * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame + */ + ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate, + double min_obstacle_height, double max_obstacle_height, double obstacle_range, + double raytrace_range, tf::TransformListener &tf, std::string global_frame, + std::string sensor_frame, double tf_tolerance); + + /** + * @brief Destructor... cleans up + */ + ~ObservationBuffer(); + + /** + * @brief Sets the global frame of an observation buffer. This will + * transform all the currently cached observations to the new global + * frame + * @param new_global_frame The name of the new global frame. + * @return True if the operation succeeds, false otherwise + */ + bool SetGlobalFrame(const std::string new_global_frame); + + /** + * @brief Transforms a PointCloud to the global frame and buffers it + * @param cloud The cloud to be buffered + */ + void BufferCloud(const sensor_msgs::PointCloud2 &cloud); + + /** + * @brief Transforms a PointCloud to the global frame and buffers it + * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier + * @param cloud The cloud to be buffered + */ + void BufferCloud(const pcl::PointCloud &cloud); + + /** + * @brief Pushes copies of all current observations onto the end of the vector passed in + * @param observations The vector to be filled + */ + void GetObservations(std::vector &observations); + + /** + * @brief Check if the observation buffer is being update at its expected rate + * @return True if it is being updated at the expected rate, false otherwise + */ + bool IsCurrent() const; + + /** + * @brief Lock the observation buffer + */ + inline void Lock() { + lock_.lock(); + } + + /** + * @brief Lock the observation buffer + */ + inline void Unlock() { + lock_.unlock(); + } + + /** + * @brief Reset last updated timestamp + */ + void ResetLastUpdated(); + + private: + /** + * @brief Removes any stale observations from the buffer list + */ + void PurgeStaleObservations(); + + tf::TransformListener &tf_; + const ros::Duration observation_keep_time_; + const ros::Duration expected_update_rate_; + ros::Time last_updated_; + std::string global_frame_; + std::string sensor_frame_; + std::list observation_list_; + std::string topic_name_; + double min_obstacle_height_, max_obstacle_height_; + std::recursive_mutex lock_; + double obstacle_range_, raytrace_range_; + double tf_tolerance_; +}; + +}// namespace roborts_costmap +#endif //ROBORTS_COSTMAP_OBSERVATION_BUFFER_H diff --git a/roborts_costmap/include/costmap/obstacle_layer.h b/roborts_costmap/include/costmap/obstacle_layer.h new file mode 100755 index 00000000..cc7a17a3 --- /dev/null +++ b/roborts_costmap/include/costmap/obstacle_layer.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_OBSTACLE_LAYER_H +#define ROBORTS_COSTMAP_OBSTACLE_LAYER_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "footprint.h" +#include "map_common.h" +#include "costmap_layer.h" +#include "layered_costmap.h" +#include "observation_buffer.h" +#include "map_common.h" + +namespace roborts_costmap { + +class ObstacleLayer : public CostmapLayer { + public: + ObstacleLayer() { + costmap_ = nullptr; + } + + virtual ~ObstacleLayer() {} + virtual void OnInitialize(); + virtual void Activate(); + virtual void Deactivate(); + virtual void Reset(); + virtual void UpdateCosts(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j); + virtual void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, + double *max_x, double *max_y) override; + void LaserScanCallback(const sensor_msgs::LaserScanConstPtr &message, + const std::shared_ptr &buffer); + void LaserScanValidInfoCallback(const sensor_msgs::LaserScanConstPtr &message, + const std::shared_ptr &buffer); + + protected: + bool GetMarkingObservations(std::vector &marking_observations) const; + bool GetClearingObservations(std::vector &clearing_observations) const; + virtual void RaytraceFreespace(const Observation &clearing_observation, double *min_x, double *min_y, + double *max_x, double *max_y); + void UpdateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double *min_x, double *min_y, + double *max_x, double *max_y); + void UpdateFootprint(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, + double *max_x, double *max_y); + bool footprint_clearing_enabled_, rolling_window_; + int combination_method_; + std::string global_frame_; + double max_obstacle_height_; + std::vector transformed_footprint_; + laser_geometry::LaserProjection projector_; + + std::vector > observation_subscribers_; + std::vector > observation_notifiers_; + std::vector > observation_buffers_; + std::vector > marking_buffers_; + std::vector > clearing_buffers_; + + std::vector static_clearing_observations_, static_marking_observations_; + std::chrono::system_clock::time_point reset_time_; +}; + +} //namespace roborts_costmap + + +#endif //ROBORTS_COSTMAP_OBSTACLE_LAYER_H diff --git a/roborts_costmap/include/costmap/static_layer.h b/roborts_costmap/include/costmap/static_layer.h new file mode 100755 index 00000000..7feecfba --- /dev/null +++ b/roborts_costmap/include/costmap/static_layer.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#ifndef ROBORTS_COSTMAP_STATIC_LAYER_H +#define ROBORTS_COSTMAP_STATIC_LAYER_H + +#include +#include "io/io.h" +#include "map_common.h" +#include "costmap_layer.h" + +namespace roborts_costmap { + +class StaticLayer : public CostmapLayer { + + public: + StaticLayer() {} + virtual ~StaticLayer() {} + virtual void OnInitialize(); + virtual void Activate(); + virtual void Deactivate(); + virtual void Reset(); + virtual void UpdateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + virtual void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + virtual void MatchSize(); + + private: + void InComingMap(const nav_msgs::OccupancyGridConstPtr& new_map); +// void IncomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update); + unsigned char InterpretValue(unsigned char value); + std::string global_frame_; + std::string map_frame_; + std::string map_topic_; + bool subscribe_to_updates_; + bool map_received_; + bool has_updated_data_; + unsigned int staic_layer_x_, staic_layer_y_, width_, height_; + unsigned char lethal_threshold_, unknown_cost_value_; + bool track_unknown_space_; + bool use_maximum_; + bool first_map_only_; + bool trinary_costmap_; + ros::Subscriber map_sub_, map_update_sub_; +}; + + +} // namespace roborts_costmap +#endif diff --git a/roborts_costmap/package.xml b/roborts_costmap/package.xml new file mode 100755 index 00000000..2fb2e5b4 --- /dev/null +++ b/roborts_costmap/package.xml @@ -0,0 +1,27 @@ + + roborts_costmap + 1.0.0 + + The roborts costmap package provides costmap representation of the environment for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + roscpp + rospy + roborts_common + + actionlib + roscpp + rospy + roborts_common + + + + + diff --git a/roborts_costmap/proto/costmap_parameter_setting.proto b/roborts_costmap/proto/costmap_parameter_setting.proto new file mode 100755 index 00000000..62e62ced --- /dev/null +++ b/roborts_costmap/proto/costmap_parameter_setting.proto @@ -0,0 +1,38 @@ +syntax = "proto2"; +package roborts_costmap; + +message ParaBasic { + required bool is_raw_rosmessage = 1; + optional bool is_debug = 2; +} +message ParaCostmapInterface { + required string global_frame = 1; + required string robot_base_frame = 2; + required double footprint_padding = 3; + required double transform_tolerance = 4; + required double distance_threshold = 5; + required double map_width = 6; + required double map_height = 7; + required double map_origin_x = 8; + required double map_origin_y = 9; + required double map_resolution = 10; + required bool is_tracking_unknown = 11; + required bool is_rolling_window = 12; + required bool has_obstacle_layer = 13; + required bool has_static_layer = 14; + required string inflation_file_path = 15; + required double map_update_frequency = 16; +} +message Point { + required double x = 1; + required double y = 2; +} +message Footprint { + repeated Point point = 1; +} + +message ParaCollection { + required ParaCostmapInterface para_costmap_interface = 1; + required Footprint footprint = 2; + required ParaBasic para_basic = 3; +} diff --git a/roborts_costmap/proto/inflation_layer_setting.proto b/roborts_costmap/proto/inflation_layer_setting.proto new file mode 100755 index 00000000..ace3a4e2 --- /dev/null +++ b/roborts_costmap/proto/inflation_layer_setting.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package roborts_costmap; + +message ParaInflationLayer { + required double inflation_radius = 1; + required double cost_scaling_factor = 2; + required bool is_debug = 3; + required bool is_raw_rosmessage = 4; +} + + diff --git a/roborts_costmap/proto/obstacle_layer_setting.proto b/roborts_costmap/proto/obstacle_layer_setting.proto new file mode 100755 index 00000000..e45f0dfb --- /dev/null +++ b/roborts_costmap/proto/obstacle_layer_setting.proto @@ -0,0 +1,18 @@ +syntax = "proto2"; +package roborts_costmap; +message ParaObstacleLayer { + required double observation_keep_time = 1; + required double expected_update_rate = 2; + required double min_obstacle_height = 3; + required double max_obstacle_height = 4; + required double obstacle_range = 5; + required double raytrace_range = 6; + required double transform_tolerance = 7; + required string topic_string = 8; + required string sensor_frame = 9; + required bool inf_is_valid = 10; + required bool clearing = 11; + required bool marking = 12; + required bool footprint_clearing_enabled = 13; + required bool is_debug = 14; +} diff --git a/roborts_costmap/proto/static_layer_setting.proto b/roborts_costmap/proto/static_layer_setting.proto new file mode 100755 index 00000000..e9430dc9 --- /dev/null +++ b/roborts_costmap/proto/static_layer_setting.proto @@ -0,0 +1,15 @@ +syntax = "proto2"; +package roborts_costmap; + +message ParaStaticLayer { + required bool first_map_only = 1; + required bool subscribe_to_updates = 2; + required bool track_unknown_space = 3; + required bool use_maximum = 4; + required int32 unknown_cost_value = 5; + required bool trinary_map = 6; + required int32 lethal_threshold = 7; + required string topic_name = 8; + required bool is_raw_rosmessage = 9; + required bool is_debug = 10; +} diff --git a/roborts_costmap/src/costmap_2d.cpp b/roborts_costmap/src/costmap_2d.cpp new file mode 100755 index 00000000..46f547b5 --- /dev/null +++ b/roborts_costmap/src/costmap_2d.cpp @@ -0,0 +1,377 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include "costmap_2d.h" + +namespace roborts_costmap { + +Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, \ + double origin_y, unsigned char default_value) : size_x_(cells_size_x), + size_y_(cells_size_y), \ + + resolution_(resolution), + origin_x_(origin_x), + origin_y_(origin_y), + costmap_(NULL), + default_value_(default_value) { + access_ = new mutex_t(); + InitMaps(size_x_, size_y_); + ResetMaps(); +} + +void Costmap2D::DeleteMaps() { + std::unique_lock lock(*access_); + delete[] costmap_; + costmap_ = NULL; +} + +void Costmap2D::InitMaps(unsigned int size_x, unsigned int size_y) { + std::unique_lock lock(*access_); + delete[] costmap_; + costmap_ = new unsigned char[size_x * size_y]; +} + +void Costmap2D::ResizeMap(unsigned int size_x, + unsigned int size_y, + double resolution, + double origin_x, + double origin_y) { + size_x_ = size_x; + size_y_ = size_y; + resolution_ = resolution; + origin_x_ = origin_x; + origin_y_ = origin_y; + InitMaps(size_x, size_y); + ResetMaps(); +} + +void Costmap2D::ResetMaps() { + std::unique_lock lock(*access_); + memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); +} + +void Costmap2D::ResetPartMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn) { + std::unique_lock lock(*(access_)); + unsigned int len = xn - x0; + for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) + memset(costmap_ + y, default_value_, len * sizeof(unsigned char)); +} + +bool Costmap2D::CopyCostMapWindow(const Costmap2D &map, + double w_origin_x, + double w_origin_y, + double w_size_x, + double w_size_y) { + if (this == &map) + return false; + DeleteMaps(); + unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + if (!(map.World2Map(w_origin_x, w_origin_y, lower_left_x, lower_left_y)) || !(map.World2Map(w_origin_x + w_size_x, \ + w_origin_y + w_size_y, upper_right_x, upper_right_y))) + return false; + size_x_ = upper_right_x - lower_left_x; + size_y_ = upper_right_y - lower_left_y; + resolution_ = map.resolution_; + origin_x_ = w_origin_x; + origin_y_ = w_origin_y; + InitMaps(size_x_, size_y_); + CopyMapRegion(map.costmap_, costmap_, map.size_x_, size_x_, lower_left_x, lower_left_y, 0, 0, size_x_, size_y_); + return true; +} + +Costmap2D &Costmap2D::operator=(const Costmap2D &map) { + // check for self assignement + if (this == &map) + return *this; + DeleteMaps(); + size_x_ = map.size_x_; + size_y_ = map.size_y_; + resolution_ = map.resolution_; + origin_x_ = map.origin_x_; + origin_y_ = map.origin_y_; + InitMaps(size_x_, size_y_); + memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); + return *this; +} + +Costmap2D::Costmap2D(const Costmap2D &map) : + costmap_(NULL) { + access_ = new mutex_t(); + *this = map; +} + +Costmap2D::Costmap2D() : + size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) { + access_ = new mutex_t(); +} + +Costmap2D::~Costmap2D() { + DeleteMaps(); + delete access_; +} + +unsigned int Costmap2D::World2Cell(double world_dist) { + double cells_dist = std::max(0.0, ceil(world_dist / resolution_)); + return (unsigned int) cells_dist; +} + +unsigned char *Costmap2D::GetCharMap() const { + return costmap_; +} + +unsigned char Costmap2D::GetCost(unsigned int mx, unsigned int my) const { + return costmap_[GetIndex(mx, my)]; +} + +void Costmap2D::SetCost(unsigned int mx, unsigned int my, unsigned char cost) { + costmap_[GetIndex(mx, my)] = cost; +} + +void Costmap2D::Map2World(unsigned int mx, unsigned int my, double &wx, double &wy) const { + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; +} + +bool Costmap2D::World2Map(double wx, double wy, unsigned int &mx, unsigned int &my) const { + if (wx < origin_x_ || wy < origin_y_) { + return false; + } + mx = (int) ((wx - origin_x_) / resolution_); + my = (int) ((wy - origin_y_) / resolution_); + if (mx < size_x_ && my < size_y_) { + return true; + } + return false; +} + +void Costmap2D::World2MapNoBoundary(double world_x, + double world_y, + int &cell_x, + int &cell_y) const { + cell_x = (int) ((world_x - origin_x_) / resolution_); + cell_y = (int) ((world_y - origin_y_) / resolution_); +} + +void Costmap2D::World2MapWithBoundary(double wx, double wy, int &mx, int &my) const { + if (wx < origin_x_) { + mx = 0; + } else if (wx > resolution_ * size_x_ + origin_x_) { + mx = size_x_ - 1; + } else { + mx = (int) ((wx - origin_x_) / resolution_); + } + + if (wy < origin_y_) { + my = 0; + } else if (wy > resolution_ * size_y_ + origin_y_) { + my = size_y_ - 1; + } else { + my = (int) ((wy - origin_y_) / resolution_); + } +} + +void Costmap2D::UpdateOrigin(double new_origin_x, double new_origin_y) { + int cell_ox, cell_oy; + cell_ox = int((new_origin_x - origin_x_) / resolution_); + cell_oy = int((new_origin_y - origin_y_) / resolution_); + double new_grid_ox, new_grid_oy; + new_grid_ox = origin_x_ + cell_ox * resolution_; + new_grid_oy = origin_y_ + cell_oy * resolution_; + int size_x = size_x_; + int size_y = size_y_; + int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + lower_left_x = std::min(std::max(cell_ox, 0), size_x); + lower_left_y = std::min(std::max(cell_oy, 0), size_y); + upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x); + upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y); + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + unsigned char *local_map = new unsigned char[cell_size_x * cell_size_y]; + CopyMapRegion(costmap_, local_map, size_x_, cell_size_x, lower_left_x, lower_left_y, 0, 0, cell_size_x, cell_size_y); + ResetMaps(); + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + int start_x = lower_left_x - cell_ox; + int start_y = lower_left_y - cell_oy; + CopyMapRegion(local_map, costmap_, cell_size_x, size_x_, 0, 0, start_x, start_y, cell_size_x, cell_size_y); + delete[] local_map; +} + +bool Costmap2D::SetConvexRegionCost(const std::vector &polygon, unsigned char cost_value) { + std::vector map_polygon; + for (unsigned int i = 0; i < polygon.size(); ++i) { + MapLocation loc; + if (!World2Map(polygon[i].x, polygon[i].y, loc.x, loc.y)) { + return false; + } + map_polygon.push_back(loc); + } + std::vector polygon_cells; + FillConvexCells(map_polygon, polygon_cells); + for (unsigned int i = 0; i < polygon_cells.size(); ++i) { + unsigned int index = GetIndex(polygon_cells[i].x, polygon_cells[i].y); + costmap_[index] = cost_value; + } + return true; +} + +void Costmap2D::GetConvexEdge(const std::vector &polygon, std::vector &polygon_cells) { + PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); + for (unsigned int i = 0; i < polygon.size() - 1; ++i) { + RaytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); + } + if (!polygon.empty()) { + unsigned int last_index = polygon.size() - 1; + RaytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y); + } +} + +void Costmap2D::FillConvexCells(const std::vector &polygon, std::vector &polygon_cells) { + if (polygon.size() < 3) + return; + GetConvexEdge(polygon, polygon_cells); + MapLocation swap; + unsigned int i = 0; + while (i < polygon_cells.size() - 1) { + if (polygon_cells[i].x > polygon_cells[i + 1].x) { + swap = polygon_cells[i]; + polygon_cells[i] = polygon_cells[i + 1]; + polygon_cells[i + 1] = swap; + + if (i > 0) + --i; + } else + ++i; + } + + i = 0; + MapLocation min_pt; + MapLocation max_pt; + unsigned int min_x = polygon_cells[0].x; + unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x; + for (unsigned int x = min_x; x <= max_x; ++x) { + if (i >= polygon_cells.size() - 1) + break; + + if (polygon_cells[i].y < polygon_cells[i + 1].y) { + min_pt = polygon_cells[i]; + max_pt = polygon_cells[i + 1]; + } else { + min_pt = polygon_cells[i + 1]; + max_pt = polygon_cells[i]; + } + + i += 2; + while (i < polygon_cells.size() && polygon_cells[i].x == x) { + if (polygon_cells[i].y < min_pt.y) + min_pt = polygon_cells[i]; + else if (polygon_cells[i].y > max_pt.y) + max_pt = polygon_cells[i]; + ++i; + } + + MapLocation pt; + for (unsigned int y = min_pt.y; y < max_pt.y; ++y) { + pt.x = x; + pt.y = y; + polygon_cells.push_back(pt); + } + } +} + +unsigned int Costmap2D::GetSizeXCell() const { + return size_x_; +} + +unsigned int Costmap2D::GetSizeYCell() const { + return size_y_; +} + +double Costmap2D::GetSizeXWorld() const { + return (size_x_ - 1 + 0.5) * resolution_; +} + +double Costmap2D::GetSizeYWorld() const { + return (size_y_ - 1 + 0.5) * resolution_; +} + +double Costmap2D::GetOriginX() const { + return origin_x_; +} + +double Costmap2D::GetOriginY() const { + return origin_y_; +} + +double Costmap2D::GetResolution() const { + return resolution_; +} + +bool Costmap2D::SaveMap(std::string file_name) { + FILE *fp = fopen(file_name.c_str(), "w"); + + if (!fp) { + return false; + } + fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff); + for (unsigned int iy = 0; iy < size_y_; iy++) { + for (unsigned int ix = 0; ix < size_x_; ix++) { + unsigned char cost = GetCost(ix, iy); + fprintf(fp, "%d ", cost); + } + fprintf(fp, "\n"); + } + fclose(fp); + return true; +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/costmap_interface.cpp b/roborts_costmap/src/costmap_interface.cpp new file mode 100755 index 00000000..4faac6dc --- /dev/null +++ b/roborts_costmap/src/costmap_interface.cpp @@ -0,0 +1,474 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "costmap_parameter_setting.pb.h" +#include "costmap_interface.h" + +namespace roborts_costmap { + +CostmapInterface::CostmapInterface(std::string map_name, + tf::TransformListener &tf, + std::string config_file) : + layered_costmap_(nullptr), + name_(map_name), + tf_(tf), + config_file_(config_file), + stop_updates_(false), + initialized_(true), + stopped_(false), + robot_stopped_(false), + map_update_thread_(NULL), + last_publish_(0), + dist_behind_robot_threshold_to_care_obstacles_(0.05), + is_debug_(false), + map_update_thread_shutdown_(false) { + std::string tf_error; + ros::NodeHandle private_nh(map_name); + LoadParameter(); + layered_costmap_ = new CostmapLayers(global_frame_, is_rolling_window_, is_track_unknown_); + layered_costmap_->SetFilePath(config_file_inflation_); + ros::Time last_error = ros::Time::now(); + while (ros::ok() && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), \ + ros::Duration(0.01), &tf_error)) { + ros::spinOnce(); + if (last_error + ros::Duration(5.0) < ros::Time::now()) { + last_error = ros::Time::now(); + } + tf_error.clear(); + } + if (has_static_layer_) { + Layer *plugin_static_layer; + plugin_static_layer = new StaticLayer; + layered_costmap_->AddPlugin(plugin_static_layer); + plugin_static_layer->Initialize(layered_costmap_, map_name + "/" + "static_layer", &tf_); + } + if (has_obstacle_layer_) { + Layer *plugin_obstacle_layer = new ObstacleLayer; + layered_costmap_->AddPlugin(plugin_obstacle_layer); + plugin_obstacle_layer->Initialize(layered_costmap_, map_name + "/" + "obstacle_layer", &tf_); + } + Layer *plugin_inflation_layer = new InflationLayer; + layered_costmap_->AddPlugin(plugin_inflation_layer); + plugin_inflation_layer->Initialize(layered_costmap_, map_name + "/" + "inflation_layer", &tf_); + SetUnpaddedRobotFootprint(footprint_points_); + stop_updates_ = false; + initialized_ = true; + stopped_ = false; + robot_stopped_ = false; + map_update_thread_shutdown_ = false; + timer_ = private_nh.createTimer(ros::Duration(0.1), &CostmapInterface::DetectMovement, this); + if (is_debug_) { + // special values: + cost_translation_table_[0] = 0; // NO obstacle + cost_translation_table_[253] = 99; // INSCRIBED obstacle + cost_translation_table_[254] = 100; // LETHAL obstacle + cost_translation_table_[255] = -1; // UNKNOWN + + // regular cost values scale the range 1 to 252 (inclusive) to fit + // into 1 to 98 (inclusive). + for (int i = 1; i < 253; i++) { + cost_translation_table_[i] = char(1 + (97 * (i - 1)) / 251); + } + } + costmap_pub_ = private_nh.advertise(name_ + "/costmap", 10); + map_update_thread_ = new std::thread(std::bind(&CostmapInterface::MapUpdateLoop, this, map_update_frequency_)); + if (is_rolling_window_) { + layered_costmap_->ResizeMap((unsigned int) map_width_ / map_resolution_, + (unsigned int) map_height_ / map_resolution_, + map_resolution_, + map_origin_x_, + map_origin_y_); + } +} + +void CostmapInterface::LoadParameter() { + + ParaCollection ParaCollectionConfig; + roborts_common::ReadProtoFromTextFile(config_file_.c_str(), \ + &ParaCollectionConfig); + + + config_file_ = ros::package::getPath("roborts_costmap") + \ + ParaCollectionConfig.para_costmap_interface().inflation_file_path(); + + map_update_frequency_ = ParaCollectionConfig.para_costmap_interface().map_update_frequency(); + global_frame_ = ParaCollectionConfig.para_costmap_interface().global_frame(); + robot_base_frame_ = ParaCollectionConfig.para_costmap_interface().robot_base_frame(); + footprint_padding_ = ParaCollectionConfig.para_costmap_interface().footprint_padding(); + transform_tolerance_ = ParaCollectionConfig.para_costmap_interface().transform_tolerance(); + is_rolling_window_ = ParaCollectionConfig.para_costmap_interface().is_rolling_window(); + is_debug_ = ParaCollectionConfig.para_basic().is_debug(); + is_track_unknown_ = ParaCollectionConfig.para_costmap_interface().is_tracking_unknown(); + has_obstacle_layer_ = ParaCollectionConfig.para_costmap_interface().has_obstacle_layer(); + has_static_layer_ = ParaCollectionConfig.para_costmap_interface().has_static_layer(); + map_width_ = ParaCollectionConfig.para_costmap_interface().map_width(); + map_height_ = ParaCollectionConfig.para_costmap_interface().map_height(); + map_origin_x_ = ParaCollectionConfig.para_costmap_interface().map_origin_x(); + map_origin_y_ = ParaCollectionConfig.para_costmap_interface().map_origin_y(); + map_resolution_ = ParaCollectionConfig.para_costmap_interface().map_resolution(); + + + config_file_inflation_ = ros::package::getPath("roborts_costmap") + \ + ParaCollectionConfig.para_costmap_interface().inflation_file_path(); + + geometry_msgs::Point point; + for (auto i = 0; i < ParaCollectionConfig.footprint().point().size(); i++) { + point.x = ParaCollectionConfig.footprint().point(i).x(); + point.y = ParaCollectionConfig.footprint().point(i).y(); + point.z = 0.0; + footprint_points_.push_back(point); + } +} + +void CostmapInterface::SetUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon &footprint) { + SetUnpaddedRobotFootprint(ToPointVector(footprint)); +} + +CostmapInterface::~CostmapInterface() { + timer_.stop(); + map_update_thread_shutdown_ = true; + if (map_update_thread_ != NULL) { + map_update_thread_->join(); + delete map_update_thread_; + } + delete layered_costmap_; +} + +void CostmapInterface::SetUnpaddedRobotFootprint(const std::vector &points) { + unpadded_footprint_ = points; + padded_footprint_ = points; + PadFootprint(padded_footprint_, footprint_padding_); + layered_costmap_->SetFootprint(padded_footprint_); +} + +void CostmapInterface::DetectMovement(const ros::TimerEvent &event) { + tf::Stamped new_pose; + if (!GetRobotPose(new_pose)) { + robot_stopped_ = false; + } else if (fabs((old_pose_.getOrigin() - new_pose.getOrigin()).length()) < 1e-3 \ + && fabs(old_pose_.getRotation().angle(new_pose.getRotation())) < 1e-3) { + old_pose_ = new_pose; + robot_stopped_ = true; + } else { + old_pose_ = new_pose; + robot_stopped_ = false; + } +} + +void CostmapInterface::MapUpdateLoop(double frequency) { + if (frequency <= 0.0) { + ROS_ERROR("Frequency must be positive in MapUpdateLoop."); + } + ros::NodeHandle nh; + ros::Rate r(frequency); + while (nh.ok() && !map_update_thread_shutdown_) { + struct timeval start, end; + gettimeofday(&start, NULL); + UpdateMap(); + gettimeofday(&end, NULL); + r.sleep(); + if (r.cycleTime() > ros::Duration(1 / frequency)) { + if (is_debug_) { + ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds",\ + frequency, r.cycleTime().toSec()); + } + } + double x, y; + Costmap2D *temp_costmap = layered_costmap_->GetCostMap(); + unsigned char *data = temp_costmap->GetCharMap(); + temp_costmap->Map2World(0, 0, x, y); + grid_.header.frame_id = global_frame_; + grid_.header.stamp = ros::Time::now(); + if (is_rolling_window_) { + grid_.info.resolution = map_resolution_; + grid_.info.width = map_width_ / map_resolution_; + grid_.info.height = map_height_ / map_resolution_; + grid_.info.origin.position.x = x - map_resolution_ * 0.5; + grid_.info.origin.position.y = y - map_resolution_ * 0.5; + grid_.info.origin.position.z = 0; + grid_.info.origin.orientation.w = 1.0; + grid_.data.resize(grid_.info.width * grid_.info.height); + } else { + auto resolution = temp_costmap->GetResolution(); + auto map_width = temp_costmap->GetSizeXCell(); + auto map_height = temp_costmap->GetSizeYCell(); + grid_.info.resolution = resolution; + grid_.info.width = map_width; + grid_.info.height = map_height; + grid_.info.origin.position.x = temp_costmap->GetOriginX(); + grid_.info.origin.position.y = temp_costmap->GetOriginY(); + grid_.info.origin.position.z = 0; + grid_.info.origin.orientation.w = 1.0; + grid_.data.resize(map_width * map_height); + } + for (size_t i = 0; i < grid_.data.size(); i++) { + grid_.data[i] = cost_translation_table_[data[i]]; + } + costmap_pub_.publish(grid_); + } +} + +void CostmapInterface::UpdateMap() { + if (!stop_updates_) { + tf::Stamped pose; + if (GetRobotPose(pose)) { + double x = pose.getOrigin().x(), y = pose.getOrigin().y(), \ + yaw = tf::getYaw(pose.getRotation()); + layered_costmap_->UpdateMap(x, y, yaw); + geometry_msgs::PolygonStamped footprint; + footprint.header.frame_id = global_frame_; + footprint.header.stamp = ros::Time::now(); + TransformFootprint(x, y, yaw, padded_footprint_, footprint); + initialized_ = true; + } + } +} + +void CostmapInterface::Start() { + auto *plugins = layered_costmap_->GetPlugins(); + if (stopped_) { + for (auto plugin = plugins->begin(); plugin != plugins->end(); ++plugin) { + (*plugin)->Activate(); + } + stopped_ = false; + } + stop_updates_ = false; + ros::Rate r(100.0); + while (ros::ok() && !initialized_) { + r.sleep(); + } +} + +void CostmapInterface::Stop() { + stop_updates_ = true; + auto *plugins = layered_costmap_->GetPlugins(); + for (auto plugin = plugins->begin(); plugin != plugins->end(); ++plugin) { + (*plugin)->Deactivate(); + } + initialized_ = false; + stopped_ = true; +} + +void CostmapInterface::Pause() { + stop_updates_ = true; + initialized_ = false; +} + +void CostmapInterface::Resume() { + stop_updates_ = false; + ros::Rate r(100.0); + while (!initialized_) { + r.sleep(); + } +} + +void CostmapInterface::ResetLayers() { + Costmap2D *master = layered_costmap_->GetCostMap(); + master->ResetPartMap(0, 0, master->GetSizeXCell(), master->GetSizeYCell()); + auto plugins = layered_costmap_->GetPlugins(); + for (auto plugin = (*plugins).begin(); plugin != (*plugins).end(); ++plugin) { + (*plugin)->Reset(); + } +} + +bool CostmapInterface::GetRobotPose(tf::Stamped &global_pose) const { + global_pose.setIdentity(); + tf::Stamped robot_pose; + robot_pose.setIdentity(); + robot_pose.frame_id_ = robot_base_frame_; + robot_pose.stamp_ = ros::Time(); + ros::Time current_time = ros::Time::now(); + try { + tf_.transformPose(global_frame_, robot_pose, global_pose); + } + catch (tf::LookupException &ex) { + ROS_ERROR("No Transform Error looking up robot pose: %s", ex.what()); + return false; + } + catch (tf::ConnectivityException &ex) { + ROS_ERROR("Connectivity Error looking up robot pose: %s", ex.what()); + return false; + } + catch (tf::ExtrapolationException &ex) { + ROS_ERROR("Extrapolation Error looking up robot pose: %s", ex.what()); + return false; + } + // check global_pose timeout + //if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) { + // LOG_WARNING << "Interface transform timeout. Current time: " << current_time.toSec() << ", global_pose stamp: " + // << global_pose.stamp_.toSec() << ", tolerance: " << transform_tolerance_; + // return false; + //} + return true; +} + +bool CostmapInterface::GetRobotPose(geometry_msgs::PoseStamped &global_pose) const { + tf::Stamped tf_global_pose; + if (GetRobotPose(tf_global_pose)) { + tf::poseStampedTFToMsg(tf_global_pose, global_pose); + return true; + } else { + return false; + } +} + +void CostmapInterface::GetOrientedFootprint(std::vector &oriented_footprint) const { + tf::Stamped global_pose; + if (!GetRobotPose(global_pose)) { + return; + } + double yaw = tf::getYaw(global_pose.getRotation()); + TransformFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw, + padded_footprint_, oriented_footprint); +} + +void CostmapInterface::GetFootprint(std::vector &footprint) { + std::vector ros_footprint = GetRobotFootprint(); + Eigen::Vector3f point; + for (auto it: ros_footprint) { + point << it.x, it.y, it.z; + footprint.push_back(point); + } +} + +void CostmapInterface::GetOrientedFootprint(std::vector &footprint) { + std::vector oriented_footprint; + GetOrientedFootprint(oriented_footprint); + Eigen::Vector3f position; + for (auto it: oriented_footprint) { + position << it.x, it.y, it.z; + footprint.push_back(position); + } +} + +bool CostmapInterface::GetRobotPose(RobotPose &pose) { + tf::Stamped global_pose; + if (GetRobotPose(global_pose)) { + pose.time = global_pose.stamp_; + pose.frame_id = global_pose.frame_id_; + pose.position << global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), global_pose.getOrigin().getZ(); + auto mat = global_pose.getBasis(); + pose.rotation << mat.getRow(0).getX(), mat.getRow(0).getY(), mat.getRow(0).getZ(), \ + mat.getRow(1).getX(), mat.getRow(1).getY(), mat.getRow(1).getZ(), \ + mat.getRow(2).getX(), mat.getRow(2).getY(), mat.getRow(2).getZ(); + return true; + } + return false; +} + +geometry_msgs::PoseStamped CostmapInterface::Pose2GlobalFrame(const geometry_msgs::PoseStamped &pose_msg) { + tf::Stamped tf_pose, global_tf_pose; + poseStampedMsgToTF(pose_msg, tf_pose); + + tf_pose.stamp_ = ros::Time(); + try { + tf_.transformPose(global_frame_, tf_pose, global_tf_pose); + } + catch (tf::TransformException &ex) { + return pose_msg; + } + geometry_msgs::PoseStamped global_pose_msg; + tf::poseStampedTFToMsg(global_tf_pose, global_pose_msg); + return global_pose_msg; +} + +void CostmapInterface::ClearCostMap() { + std::vector *plugins = layered_costmap_->GetPlugins(); + tf::Stamped pose; + if (GetRobotPose(pose) == false) { + return; + } + double pose_x = pose.getOrigin().x(); + double pose_y = pose.getOrigin().y(); + + for (std::vector::iterator plugin_iter = plugins->begin(); + plugin_iter != plugins->end(); + ++plugin_iter) { + roborts_costmap::Layer *plugin = *plugin_iter; + if (plugin->GetName().find("obstacle") != std::string::npos) { + CostmapLayer *costmap_layer_ptr = (CostmapLayer *) plugin; + ClearLayer(costmap_layer_ptr, pose_x, pose_y); + } + + } +} + +void CostmapInterface::ClearLayer(CostmapLayer *costmap_layer_ptr, double pose_x, double pose_y) { + std::unique_lock lock(*(costmap_layer_ptr->GetMutex())); + double reset_distance = 0.1; + double start_point_x = pose_x - reset_distance / 2; + double start_point_y = pose_y - reset_distance / 2; + double end_point_x = start_point_x + reset_distance; + double end_point_y = start_point_y + reset_distance; + + int start_x, start_y, end_x, end_y; + costmap_layer_ptr->World2MapNoBoundary(start_point_x, start_point_y, start_x, start_y); + costmap_layer_ptr->World2MapNoBoundary(end_point_x, end_point_y, end_x, end_y); + + unsigned char *grid = costmap_layer_ptr->GetCharMap(); + for (int x = 0; x < (int) costmap_layer_ptr->GetSizeXCell(); x++) { + bool xrange = x > start_x && x < end_x; + + for (int y = 0; y < (int) costmap_layer_ptr->GetSizeYCell(); y++) { + if (xrange && y > start_y && y < end_y) + continue; + int index = costmap_layer_ptr->GetIndex(x, y); + if (grid[index] != NO_INFORMATION) { + grid[index] = NO_INFORMATION; + } + } + } + + double ox = costmap_layer_ptr->GetOriginX(), oy = costmap_layer_ptr->GetOriginY(); + double width = costmap_layer_ptr->GetSizeXWorld(), height = costmap_layer_ptr->GetSizeYWorld(); + costmap_layer_ptr->AddExtraBounds(ox, oy, ox + width, oy + height); +} +} //namespace roborts_costmap diff --git a/roborts_costmap/src/costmap_layer.cpp b/roborts_costmap/src/costmap_layer.cpp new file mode 100755 index 00000000..fb1cc7d5 --- /dev/null +++ b/roborts_costmap/src/costmap_layer.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "costmap_layer.h" + +namespace roborts_costmap { + +void CostmapLayer::Touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y) { + *min_x = std::min(x, *min_x); + *min_y = std::min(y, *min_y); + *max_x = std::max(x, *max_x); + *max_y = std::max(y, *max_y); +} + +void CostmapLayer::MatchSize() { + Costmap2D *master = layered_costmap_->GetCostMap(); + ResizeMap(master->GetSizeXCell(), master->GetSizeYCell(), master->GetResolution(), + master->GetOriginX(), master->GetOriginY()); +} + +void CostmapLayer::AddExtraBounds(double mx0, double my0, double mx1, double my1) { + extra_min_x_ = std::min(mx0, extra_min_x_); + extra_max_x_ = std::max(mx1, extra_max_x_); + extra_min_y_ = std::min(my0, extra_min_y_); + extra_max_y_ = std::max(my1, extra_max_y_); + has_extra_bounds_ = true; +} + +void CostmapLayer::UseExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y) { + if (!has_extra_bounds_) + return; + + *min_x = std::min(extra_min_x_, *min_x); + *min_y = std::min(extra_min_y_, *min_y); + *max_x = std::max(extra_max_x_, *max_x); + *max_y = std::max(extra_max_y_, *max_y); + extra_min_x_ = 1e6; + extra_min_y_ = 1e6; + extra_max_x_ = -1e6; + extra_max_y_ = -1e6; + has_extra_bounds_ = false; +} + +void CostmapLayer::UpdateOverwriteByMax(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { + if (!is_enabled_) + return; + + unsigned char *master_array = master_grid.GetCharMap(); + unsigned int span = master_grid.GetSizeXCell(); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) { + if (costmap_[it] == NO_INFORMATION) { + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + if (old_cost == NO_INFORMATION || old_cost < costmap_[it]) + master_array[it] = costmap_[it]; + it++; + } + } +} + +void CostmapLayer::UpdateOverwriteByAll(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { + if (!is_current_) + return; + unsigned char *master = master_grid.GetCharMap(); + unsigned int span = master_grid.GetSizeXCell(); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = span * j + min_i; + for (int i = min_i; i < max_i; i++) { + master[it] = costmap_[it]; + it++; + } + } +} + +void CostmapLayer::UpdateOverwriteByValid(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { + if (!is_enabled_) + return; + unsigned char *master = master_grid.GetCharMap(); + unsigned int span = master_grid.GetSizeXCell(); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = span * j + min_i; + for (int i = min_i; i < max_i; i++) { + if (costmap_[it] != NO_INFORMATION) + master[it] = costmap_[it]; + it++; + } + } +} + +void CostmapLayer::UpdateOverwriteByAdd(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { + if (!is_enabled_) + return; + unsigned char *master_array = master_grid.GetCharMap(); + unsigned int span = master_grid.GetSizeXCell(); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) { + if (costmap_[it] == NO_INFORMATION) { + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + if (old_cost == NO_INFORMATION) + master_array[it] = costmap_[it]; + else { + int sum = old_cost + costmap_[it]; + if (sum >= INSCRIBED_INFLATED_OBSTACLE) + master_array[it] = INSCRIBED_INFLATED_OBSTACLE - 1; + else + master_array[it] = sum; + } + it++; + } + } +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/costmap_math.cpp b/roborts_costmap/src/costmap_math.cpp new file mode 100755 index 00000000..a6346074 --- /dev/null +++ b/roborts_costmap/src/costmap_math.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "costmap_math.h" +namespace roborts_costmap { +double Distance2Line(double pX, double pY, double x0, double y0, double x1, double y1) { + double A = pX - x0; + double B = pY - y0; + double C = x1 - x0; + double D = y1 - y0; + + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; + + double xx, yy; + + if (param < 0) { + xx = x0; + yy = y0; + } else if (param > 1) { + xx = x1; + yy = y1; + } else { + xx = x0 + param * C; + yy = y0 + param * D; + } + + return distance(pX, pY, xx, yy); +} + +bool Intersect(std::vector &polygon, float testx, float testy) { + bool c = false; + int i, j, nvert = polygon.size(); + for (i = 0, j = nvert - 1; i < nvert; j = i++) { + float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x; + + if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi)) + c = !c; + } + return c; +} + +bool intersects_helper(std::vector &polygon1, std::vector &polygon2) { + for (unsigned int i = 0; i < polygon1.size(); i++) + if (Intersect(polygon2, polygon1[i].x, polygon1[i].y)) + return true; + return false; +} + +bool Intersect(std::vector &polygon1, std::vector &polygon2) { + return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1); +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/footprint.cpp b/roborts_costmap/src/footprint.cpp new file mode 100755 index 00000000..99227f30 --- /dev/null +++ b/roborts_costmap/src/footprint.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include +#include +#include "costmap_math.h" +#include "footprint.h" + +namespace roborts_costmap { + +void CalculateMinAndMaxDistances(const std::vector &footprint, + double &min_dist, + double &max_dist) { + min_dist = std::numeric_limits::max(); + max_dist = 0.0; + + if (footprint.size() <= 2) { + return; + } + + for (unsigned int i = 0; i < footprint.size() - 1; ++i) { + // check the distance from the robot center point to the first vertex + double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y); + double edge_dist = Distance2Line(0.0, 0.0, footprint[i].x, footprint[i].y, + footprint[i + 1].x, footprint[i + 1].y); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); + } + + // we also need to do the last vertex and the first vertex + double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y); + double edge_dist = Distance2Line(0.0, 0.0, footprint.back().x, footprint.back().y, + footprint.front().x, footprint.front().y); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); +} + +geometry_msgs::Point32 ToPoint32(geometry_msgs::Point pt) { + geometry_msgs::Point32 point32; + point32.x = pt.x; + point32.y = pt.y; + point32.z = pt.z; + return point32; +} + +geometry_msgs::Point ToPoint(geometry_msgs::Point32 pt) { + geometry_msgs::Point point; + point.x = pt.x; + point.y = pt.y; + point.z = pt.z; + return point; +} + +geometry_msgs::Polygon ToPolygon(std::vector pts) { + geometry_msgs::Polygon polygon; + for (int i = 0; i < pts.size(); i++) { + polygon.points.push_back(ToPoint32(pts[i])); + } + return polygon; +} + +std::vector ToPointVector(geometry_msgs::Polygon polygon) { + std::vector pts; + for (int i = 0; i < polygon.points.size(); i++) { + pts.push_back(ToPoint(polygon.points[i])); + } + return pts; +} + +void TransformFootprint(double x, double y, double theta, const std::vector &footprint_spec, + std::vector &oriented_footprint) { + // build the oriented footprint at a given location + oriented_footprint.clear(); + double cos_th = cos(theta); + double sin_th = sin(theta); + for (unsigned int i = 0; i < footprint_spec.size(); ++i) { + geometry_msgs::Point new_pt; + new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); + new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); + oriented_footprint.push_back(new_pt); + } +} + +void TransformFootprint(double x, double y, double theta, const std::vector &footprint_spec, + geometry_msgs::PolygonStamped &oriented_footprint) { + // build the oriented footprint at a given location + oriented_footprint.polygon.points.clear(); + double cos_th = cos(theta); + double sin_th = sin(theta); + for (unsigned int i = 0; i < footprint_spec.size(); ++i) { + geometry_msgs::Point32 new_pt; + new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); + new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); + oriented_footprint.polygon.points.push_back(new_pt); + } +} + +void PadFootprint(std::vector &footprint, double padding) { + // pad footprint in place + for (unsigned int i = 0; i < footprint.size(); i++) { + geometry_msgs::Point &pt = footprint[i]; + pt.x += sign0(pt.x) * padding; + pt.y += sign0(pt.y) * padding; + } +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/inflation_layer.cpp b/roborts_costmap/src/inflation_layer.cpp new file mode 100755 index 00000000..9e421667 --- /dev/null +++ b/roborts_costmap/src/inflation_layer.cpp @@ -0,0 +1,326 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include +#include +#include "costmap_math.h" +#include "footprint.h" +#include "inflation_layer.h" +#include "inflation_layer_setting.pb.h" +namespace roborts_costmap { + +InflationLayer::InflationLayer() + : inflation_radius_(0), + weight_(0), + inflate_unknown_(false), + cell_inflation_radius_(0), + cached_cell_inflation_radius_(0), + seen_(NULL), + cached_costs_(NULL), + cached_distances_(NULL), + last_min_x_(-std::numeric_limits::max()), + last_min_y_(-std::numeric_limits::max()), + last_max_x_(std::numeric_limits::max()), + last_max_y_(std::numeric_limits::max()) { + inflation_access_ = new std::recursive_mutex(); +} + +void InflationLayer::OnInitialize() { + + std::unique_lock lock(*inflation_access_); + ros::NodeHandle nh("~/" + name_), g_nh; + is_current_ = true; + if (seen_) + delete[] seen_; + seen_ = NULL; + seen_size_ = 0; + need_reinflation_ = false; + double inflation_radius, cost_scaling_factor; + ParaInflationLayer para_inflation; + roborts_common::ReadProtoFromTextFile(layered_costmap_->GetFilePath().c_str(), ¶_inflation); + inflation_radius = para_inflation.inflation_radius(); + cost_scaling_factor = para_inflation.cost_scaling_factor(); + need_reinflation_ = false; + SetInflationParameters(inflation_radius, cost_scaling_factor); + is_enabled_ = true; + inflate_unknown_ = false; + need_reinflation_ = true; + MatchSize(); +} + +void InflationLayer::MatchSize() { + std::unique_lock lock(*inflation_access_); + Costmap2D *costmap = layered_costmap_->GetCostMap(); + resolution_ = costmap->GetResolution(); + cell_inflation_radius_ = CellDistance(inflation_radius_); + ComputeCaches(); + + unsigned int size_x = costmap->GetSizeXCell(), size_y = costmap->GetSizeYCell(); + if (seen_) + delete[] seen_; + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; +} + +void InflationLayer::UpdateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, + double *min_y, double *max_x, double *max_y) { + if (need_reinflation_) { + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + // For some reason when I make these -::max() it does not + // work with Costmap2D::worldToMapEnforceBounds(), so I'm using + // -::max() instead. + *min_x = -std::numeric_limits::max(); + *min_y = -std::numeric_limits::max(); + *max_x = std::numeric_limits::max(); + *max_y = std::numeric_limits::max(); + need_reinflation_ = false; + } else { + double tmp_min_x = last_min_x_; + double tmp_min_y = last_min_y_; + double tmp_max_x = last_max_x_; + double tmp_max_y = last_max_y_; + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_; + *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_; + *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_; + *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_; + } +} + +void InflationLayer::OnFootprintChanged() { + inscribed_radius_ = layered_costmap_->GetInscribedRadius(); + cell_inflation_radius_ = CellDistance(inflation_radius_); + ComputeCaches(); + need_reinflation_ = true; +} + +void InflationLayer::UpdateCosts(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { + std::unique_lock lock(*inflation_access_); + if (!is_enabled_ || (cell_inflation_radius_ == 0)) { + ROS_ERROR("Layer is not enabled or inflation radius is zero"); + return; + } + unsigned char *master_array = master_grid.GetCharMap(); + unsigned int size_x = master_grid.GetSizeXCell(), size_y = master_grid.GetSizeYCell(); + + if (seen_ == NULL) { + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; + } else if (seen_size_ != size_x * size_y) { + delete[] seen_; + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; + } + memset(seen_, false, size_x * size_y * sizeof(bool)); + + min_i -= cell_inflation_radius_; + min_j -= cell_inflation_radius_; + max_i += cell_inflation_radius_; + max_j += cell_inflation_radius_; + + min_i = std::max(0, min_i); + min_j = std::max(0, min_j); + max_i = std::min(int(size_x), max_i); + max_j = std::min(int(size_y), max_j); + + // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle + // We use a map to emulate the priority queue used before, with a notable performance boost + + // Start with lethal obstacles: by definition distance is 0.0 + std::vector &obs_bin = inflation_cells_[0.0]; + for (int j = min_j; j < max_j; j++) { + for (int i = min_i; i < max_i; i++) { + int index = master_grid.GetIndex(i, j); + unsigned char cost = master_array[index]; + if (cost == LETHAL_OBSTACLE) { + obs_bin.push_back(CellData(index, i, j, i, j)); + } + } + } + + // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they + // can overtake previously inserted but farther away cells + std::map >::iterator bin; + for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) { + for (int i = 0; i < bin->second.size(); ++i) { + // process all cells at distance dist_bin.first + const CellData &cell = bin->second[i]; + + unsigned int index = cell.index_; + + // ignore if already visited + if (seen_[index]) { + continue; + } + + seen_[index] = true; + + unsigned int mx = cell.x_; + unsigned int my = cell.y_; + unsigned int sx = cell.src_x_; + unsigned int sy = cell.src_y_; + + // assign the cost associated with the distance from an obstacle to the cell + unsigned char cost = CostLookup(mx, my, sx, sy); + unsigned char old_cost = master_array[index]; + if (old_cost == NO_INFORMATION + && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + master_array[index] = cost; + else + master_array[index] = std::max(old_cost, cost); + + // attempt to put the neighbors of the current cell onto the inflation list + if (mx > 0) + Enqueue(index - 1, mx - 1, my, sx, sy); + if (my > 0) + Enqueue(index - size_x, mx, my - 1, sx, sy); + if (mx < size_x - 1) + Enqueue(index + 1, mx + 1, my, sx, sy); + if (my < size_y - 1) + Enqueue(index + size_x, mx, my + 1, sx, sy); + } + } + + inflation_cells_.clear(); +} + +/** + * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation + * @param grid The costmap + * @param index The index of the cell + * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) + * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) + * @param src_x The x index of the obstacle point inflation started at + * @param src_y The y index of the obstacle point inflation started at + */ +inline void InflationLayer::Enqueue(unsigned int index, unsigned int mx, unsigned int my, + unsigned int src_x, unsigned int src_y) { + if (!seen_[index]) { + // we compute our distance table one cell further than the inflation radius dictates so we can make the check below + double distance = DistanceLookup(mx, my, src_x, src_y); + + // we only want to put the cell in the list if it is within the inflation radius of the obstacle point + if (distance > cell_inflation_radius_) + return; + + // push the cell data onto the inflation list and mark + inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); + } +} + +void InflationLayer::ComputeCaches() { + if (cell_inflation_radius_ == 0) + return; + + // based on the inflation radius... compute distance and cost caches + if (cell_inflation_radius_ != cached_cell_inflation_radius_) { + DeleteKernels(); + //make a 2D array + cached_costs_ = new unsigned char *[cell_inflation_radius_ + 2]; + cached_distances_ = new double *[cell_inflation_radius_ + 2]; + + for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) { + cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; + cached_distances_[i] = new double[cell_inflation_radius_ + 2]; + for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) { + cached_distances_[i][j] = hypot(i, j); + } + } + + cached_cell_inflation_radius_ = cell_inflation_radius_; + } + + for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) { + for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) { + cached_costs_[i][j] = ComputeCost(cached_distances_[i][j]); + } + } +} + +void InflationLayer::DeleteKernels() { + if (cached_distances_ != NULL) { + for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) { + if (cached_distances_[i]) + delete[] cached_distances_[i]; + } + if (cached_distances_) + delete[] cached_distances_; + cached_distances_ = NULL; + } + + if (cached_costs_ != NULL) { + for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) { + if (cached_costs_[i]) + delete[] cached_costs_[i]; + } + delete[] cached_costs_; + cached_costs_ = NULL; + } +} + +void InflationLayer::SetInflationParameters(double inflation_radius, double cost_scaling_factor) { + if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) { + std::unique_lock lock(*inflation_access_); + inflation_radius_ = inflation_radius; + cell_inflation_radius_ = CellDistance(inflation_radius_); + weight_ = cost_scaling_factor; + need_reinflation_ = true; + ComputeCaches(); + } +} + +}//namespace roborts_costmap diff --git a/roborts_costmap/src/layer.cpp b/roborts_costmap/src/layer.cpp new file mode 100755 index 00000000..94c2b357 --- /dev/null +++ b/roborts_costmap/src/layer.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "layer.h" + +namespace roborts_costmap { + +Layer::Layer() + : layered_costmap_(NULL), is_current_(false), is_enabled_(false), name_(), tf_(NULL) {} + +void Layer::Initialize(CostmapLayers *parent, std::string name, tf::TransformListener *tf) { + layered_costmap_ = parent; + name_ = name; + tf_ = tf; + OnInitialize(); +} + +const std::vector &Layer::GetFootprint() const { + return layered_costmap_->GetFootprint(); +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/layered_costmap.cpp b/roborts_costmap/src/layered_costmap.cpp new file mode 100755 index 00000000..0fced215 --- /dev/null +++ b/roborts_costmap/src/layered_costmap.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "layered_costmap.h" + +namespace roborts_costmap { + +CostmapLayers::CostmapLayers(std::string global_frame, bool rolling_window, bool track_unknown) : costmap_(), \ + global_frame_id_(global_frame), is_rolling_window_(rolling_window), is_initialized_(false), \ + is_size_locked_(false), file_path_("") { + if (track_unknown) { + costmap_.SetDefaultValue(255); + } else { + costmap_.SetDefaultValue(0); + } +} + +CostmapLayers::~CostmapLayers() { + for (auto i = plugins_.size(); i > 0; i--) { + delete plugins_[i - 1]; + } + plugins_.clear(); +} + +bool CostmapLayers::IsCurrent() { + is_current_ = true; + for (auto it = plugins_.begin(); it != plugins_.end(); ++it) { + is_current_ = is_current_ && (*it)->IsCurrent(); + } + return is_current_; +} + +void CostmapLayers::ResizeMap(unsigned int size_x, + unsigned int size_y, + double resolution, + double origin_x, + double origin_y, + bool size_locked) { + is_size_locked_ = size_locked; + costmap_.ResizeMap(size_x, size_y, resolution, origin_x, origin_y); + for (auto it = plugins_.begin(); it != plugins_.end(); ++it) { + (*it)->MatchSize(); + } +} + +void CostmapLayers::UpdateMap(double robot_x, double robot_y, double robot_yaw) { + static int count = 0; + std::unique_lock lock(*(costmap_.GetMutex())); + if (is_rolling_window_) { + double new_origin_x = robot_x - costmap_.GetSizeXWorld() / 2; + double new_origin_y = robot_y - costmap_.GetSizeYWorld() / 2; + costmap_.UpdateOrigin(new_origin_x, new_origin_y); + } + if (plugins_.size() == 0) { + ROS_WARN("No Layer"); + return; + } + + minx_ = miny_ = 1e30; + maxx_ = maxy_ = -1e30; + for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { + double prev_minx = minx_; + double prev_miny = miny_; + double prev_maxx = maxx_; + double prev_maxy = maxy_; + (*plugin)->UpdateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + count++; + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { + ROS_WARN("Illegal bounds change. The offending layer is %s", (*plugin)->GetName().c_str()); + } + } + int x0, xn, y0, yn; + costmap_.World2MapWithBoundary(minx_, miny_, x0, y0); + costmap_.World2MapWithBoundary(maxx_, maxy_, xn, yn); + x0 = std::max(0, x0); + xn = std::min(int(costmap_.GetSizeXCell()), xn + 1); + y0 = std::max(0, y0); + yn = std::min(int(costmap_.GetSizeYCell()), yn + 1); + if (xn < x0 || yn < y0) { + return; + } + costmap_.ResetPartMap(x0, y0, xn, yn); + for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { + (*plugin)->UpdateCosts(costmap_, x0, y0, xn, yn); + } + + bx0_ = x0; + bxn_ = xn; + by0_ = y0; + byn_ = yn; + is_initialized_ = true; +} + +void CostmapLayers::SetFootprint(const std::vector &footprint_spec) { + footprint_ = footprint_spec; + CalculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); + for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { + (*plugin)->OnFootprintChanged(); + } +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/observation_buffer.cpp b/roborts_costmap/src/observation_buffer.cpp new file mode 100755 index 00000000..66e9bfa2 --- /dev/null +++ b/roborts_costmap/src/observation_buffer.cpp @@ -0,0 +1,270 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include +#include +#include +#include +#include +#include "observation_buffer.h" + +using namespace std; +using namespace tf; + +namespace roborts_costmap { +ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate, + double min_obstacle_height, double max_obstacle_height, double obstacle_range, + double raytrace_range, TransformListener& tf, string global_frame, + string sensor_frame, double tf_tolerance) : + tf_(tf), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate), + last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name), + min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), + obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance) +{ +} + +ObservationBuffer::~ObservationBuffer() +{ +} + +bool ObservationBuffer::SetGlobalFrame(const std::string new_global_frame) +{ + ros::Time transform_time = ros::Time::now(); + std::string tf_error; + + if (!tf_.waitForTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), + ros::Duration(0.01), &tf_error)) + { + ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), + global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); + return false; + } + + list::iterator obs_it; + for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) + { + try + { + Observation& obs = *obs_it; + + geometry_msgs::PointStamped origin; + origin.header.frame_id = global_frame_; + origin.header.stamp = transform_time; + origin.point = obs.origin_; + + // we need to transform the origin of the observation to the new global frame + tf_.transformPoint(new_global_frame, origin, origin); + obs.origin_ = origin.point; + + // we also need to transform the cloud of the observation to the new global frame + pcl_ros::transformPointCloud(new_global_frame, *obs.cloud_, *obs.cloud_, tf_); + } + catch (TransformException& ex) + { + ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(), + new_global_frame.c_str(), ex.what()); + return false; + } + } + + // now we need to update our global_frame member + global_frame_ = new_global_frame; + return true; +} + +void ObservationBuffer::BufferCloud(const sensor_msgs::PointCloud2& cloud) +{ + try + { + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::toPCL(cloud, pcl_pc2); + // Actually convert the PointCloud2 message into a type we can reason about + pcl::PointCloud < pcl::PointXYZ > pcl_cloud; + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); + BufferCloud(pcl_cloud); + } + catch (pcl::PCLException& ex) + { + ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what()); + return; + } +} + +void ObservationBuffer::BufferCloud(const pcl::PointCloud& cloud) +{ + Stamped < tf::Vector3 > global_origin; + + // create a new observation on the list to be populated + observation_list_.push_front(Observation()); + + // check whether the origin frame has been set explicitly or whether we should get it from the cloud + string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; + + try + { + // given these observations come from sensors... we'll need to store the origin pt of the sensor + Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0), + pcl_conversions::fromPCL(cloud.header).stamp, origin_frame); + tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5)); + tf_.transformPoint(global_frame_, local_origin, global_origin); + observation_list_.front().origin_.x = global_origin.getX(); + observation_list_.front().origin_.y = global_origin.getY(); + observation_list_.front().origin_.z = global_origin.getZ(); + + // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations + observation_list_.front().raytrace_range_ = raytrace_range_; + observation_list_.front().obstacle_range_ = obstacle_range_; + + pcl::PointCloud < pcl::PointXYZ > global_frame_cloud; + + // transform the point cloud + pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_); + global_frame_cloud.header.stamp = cloud.header.stamp; + + // now we need to remove observations from the cloud that are below or above our height thresholds + pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_); + unsigned int cloud_size = global_frame_cloud.points.size(); + observation_cloud.points.resize(cloud_size); + unsigned int point_count = 0; + + // copy over the points that are within our height bounds + for (unsigned int i = 0; i < cloud_size; ++i) + { + if (global_frame_cloud.points[i].z <= max_obstacle_height_ + && global_frame_cloud.points[i].z >= min_obstacle_height_) + { + observation_cloud.points[point_count++] = global_frame_cloud.points[i]; + } + } + + // resize the cloud for the number of legal points + observation_cloud.points.resize(point_count); + observation_cloud.header.stamp = cloud.header.stamp; + observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; + } + catch (TransformException& ex) + { + // if an exception occurs, we need to remove the empty observation from the list + observation_list_.pop_front(); + ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), + cloud.header.frame_id.c_str(), ex.what()); + return; + } + + // if the update was successful, we want to update the last updated time + last_updated_ = ros::Time::now(); + + // we'll also remove any stale observations from the list + PurgeStaleObservations(); +} + +// returns a copy of the observations +void ObservationBuffer::GetObservations(vector& observations) +{ + // first... let's make sure that we don't have any stale observations + PurgeStaleObservations(); + // now we'll just copy the observations for the caller + list::iterator obs_it; + for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) + { + observations.push_back(*obs_it); + } +} + +void ObservationBuffer::PurgeStaleObservations() +{ + if (!observation_list_.empty()) + { + list::iterator obs_it = observation_list_.begin(); + // if we're keeping observations for no time... then we'll only keep one observation + if (observation_keep_time_ == ros::Duration(0.0)) + { + observation_list_.erase(++obs_it, observation_list_.end()); + return; + } + + // otherwise... we'll have to loop through the observations to see which ones are stale + for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) + { + Observation& obs = *obs_it; + // check if the observation is out of date... and if it is, remove it and those that follow from the list + ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp; + if ((last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp) > observation_keep_time_) + { + observation_list_.erase(obs_it, observation_list_.end()); + return; + } + } + } +} + +bool ObservationBuffer::IsCurrent() const +{ + if (expected_update_rate_ == ros::Duration(0.0)) + return true; + + bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec(); + if (!current) + { + ROS_WARN( + "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", + topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec()); + } + return current; +} + +void ObservationBuffer::ResetLastUpdated() +{ + last_updated_ = ros::Time::now(); +} + +} //namespace roborts_costmap + diff --git a/roborts_costmap/src/obstacle_layer.cpp b/roborts_costmap/src/obstacle_layer.cpp new file mode 100755 index 00000000..20402bcd --- /dev/null +++ b/roborts_costmap/src/obstacle_layer.cpp @@ -0,0 +1,417 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "obstacle_layer_setting.pb.h" +#include "obstacle_layer.h" + +namespace roborts_costmap { + +void ObstacleLayer::OnInitialize() { + ros::NodeHandle nh; + ParaObstacleLayer para_obstacle; + + std::string obstacle_map = ros::package::getPath("roborts_costmap") + \ + "/config/obstacle_layer_config.prototxt"; + roborts_common::ReadProtoFromTextFile(obstacle_map.c_str(), ¶_obstacle); + double observation_keep_time = 0.1, expected_update_rate = 10.0, min_obstacle_height = 0.2, \ + max_obstacle_height = 0.6, obstacle_range = 2.5, raytrace_range = 3.0, transform_tolerance = 0.2; + observation_keep_time = para_obstacle.observation_keep_time(); + expected_update_rate = para_obstacle.expected_update_rate(); + transform_tolerance = para_obstacle.transform_tolerance(); + max_obstacle_height = para_obstacle.max_obstacle_height(); + min_obstacle_height = para_obstacle.min_obstacle_height(); + obstacle_range = para_obstacle.obstacle_range(); + raytrace_range = para_obstacle.raytrace_range(); + max_obstacle_height_ = max_obstacle_height; + footprint_clearing_enabled_ = para_obstacle.footprint_clearing_enabled(); + std::string topic_string = "LaserScan", sensor_frame = "laser_frame"; + topic_string = para_obstacle.topic_string(); + sensor_frame = para_obstacle.sensor_frame(); + + bool inf_is_valid = false, clearing = false, marking = true; + inf_is_valid = para_obstacle.inf_is_valid(); + clearing = para_obstacle.clearing(); + marking = para_obstacle.marking(); + rolling_window_ = layered_costmap_->IsRollingWindow(); + bool track_unknown_space = layered_costmap_->IsTrackingUnknown(); + if (track_unknown_space) { + default_value_ = NO_INFORMATION; + } else { + default_value_ = FREE_SPACE; + } + is_current_ = true; + global_frame_ = layered_costmap_->GetGlobalFrameID(); + ObstacleLayer::MatchSize(); + observation_buffers_.push_back(std::shared_ptr(new ObservationBuffer(topic_string, + observation_keep_time, + expected_update_rate, + min_obstacle_height, + max_obstacle_height, + obstacle_range, + raytrace_range, + *tf_, + global_frame_, + sensor_frame, + transform_tolerance))); + if (marking) { + marking_buffers_.push_back(observation_buffers_.back()); + } + if (clearing) { + clearing_buffers_.push_back(observation_buffers_.back()); + } + reset_time_ = std::chrono::system_clock::now(); + std::shared_ptr + > sub(new message_filters::Subscriber(nh, topic_string, 50)); + std::shared_ptr + > filter(new tf::MessageFilter(*sub, *tf_, global_frame_, 50)); + if (inf_is_valid) { + filter->registerCallback( + boost::bind(&ObstacleLayer::LaserScanValidInfoCallback, this, _1, observation_buffers_.back())); + } else { + filter->registerCallback( + boost::bind(&ObstacleLayer::LaserScanCallback, this, _1, observation_buffers_.back())); + } + observation_subscribers_.push_back(sub); + observation_notifiers_.push_back(filter); + observation_notifiers_.back()->setTolerance(ros::Duration(0.05)); + std::vector target_frames; + target_frames.push_back(global_frame_); + target_frames.push_back(sensor_frame); + observation_notifiers_.back()->setTargetFrames(target_frames); + is_enabled_ = true; +} + +void ObstacleLayer::LaserScanCallback(const sensor_msgs::LaserScanConstPtr &message, + const std::shared_ptr &buffer) { + sensor_msgs::PointCloud2 temp_cloud; + temp_cloud.header = message->header; + try { + projector_.transformLaserScanToPointCloud(temp_cloud.header.frame_id, *message, temp_cloud, *tf_); + } + catch (tf::TransformException &ex) { + projector_.projectLaser(*message, temp_cloud); + } + buffer->Lock(); + buffer->BufferCloud(temp_cloud); + buffer->Unlock(); +} + +void ObstacleLayer::LaserScanValidInfoCallback(const sensor_msgs::LaserScanConstPtr &raw_message, + const std::shared_ptr &buffer) { + float epsilon = 0.0001, range; + sensor_msgs::LaserScan message = *raw_message; + for (size_t i = 0; i < message.ranges.size(); i++) { + range = message.ranges[i]; + if (!std::isfinite(range) && range > 0) { + message.ranges[i] = message.range_max - epsilon; + } + } + sensor_msgs::PointCloud2 cloud; + cloud.header = message.header; + try { + projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); + } + catch (tf::TransformException &ex) { + ROS_ERROR("High fidelity enabled, but TF returned a transform exception to frame %s: %s", \ + global_frame_.c_str(), ex.what()); + projector_.projectLaser(message, cloud); + } + buffer->Lock(); + buffer->BufferCloud(cloud); + buffer->Unlock(); +} + +void ObstacleLayer::UpdateBounds(double robot_x, + double robot_y, + double robot_yaw, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { + if (rolling_window_) { + UpdateOrigin(robot_x - GetSizeXWorld() / 2, robot_y - GetSizeYWorld() / 2); + } else if (std::chrono::duration_cast(std::chrono::system_clock::now() - reset_time_) > std::chrono::seconds(2)){ + reset_time_ = std::chrono::system_clock::now(); + ResetMaps(); + } + if (!is_enabled_) { + ROS_ERROR("Obstacle layer is not enabled."); + return; + } + UseExtraBounds(min_x, min_y, max_x, max_y); + bool temp_is_current = true; + std::vector observations, clearing_observations; + temp_is_current = temp_is_current && GetMarkingObservations(observations); + temp_is_current = temp_is_current && GetClearingObservations(clearing_observations); + is_current_ = temp_is_current; + + // raytrace freespace + for (unsigned int i = 0; i < clearing_observations.size(); ++i) { + RaytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); + } + + for (std::vector::const_iterator it = observations.begin(); it != observations.end(); it++) { + const Observation obs = *it; + const pcl::PointCloud &cloud = *(obs.cloud_); + double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; + for (unsigned int i = 0; i < cloud.points.size(); ++i) { + double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z; + + // if the obstacle is too high or too far away from the robot we won't add it + if (pz > max_obstacle_height_) { + continue; + } + + // compute the squared distance from the hitpoint to the pointcloud's origin + double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + + (pz - obs.origin_.z) * (pz - obs.origin_.z); + + // if the point is far enough away... we won't consider it + if (sq_dist >= sq_obstacle_range) { + continue; + } + + // now we need to compute the map coordinates for the observation + unsigned int mx, my; + if (!World2Map(px, py, mx, my)) { + continue; + } + unsigned int index = GetIndex(mx, my); + costmap_[index] = LETHAL_OBSTACLE; + + Touch(px, py, min_x, min_y, max_x, max_y); + } + } + UpdateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void ObstacleLayer::UpdateCosts(Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { + if (!is_enabled_) { + ROS_WARN("Obstacle layer is not enabled"); + return; + } + + if (footprint_clearing_enabled_) { + SetConvexRegionCost(transformed_footprint_, FREE_SPACE); + } + combination_method_ = 1; + switch (combination_method_) { + case 0: // Overwrite + UpdateOverwriteByValid(master_grid, min_i, min_j, max_i, max_j); + break; + case 1: // Maximum + UpdateOverwriteByMax(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } +} + +void ObstacleLayer::Activate() { + for (size_t i = 0; i < observation_subscribers_.size(); ++i) { + if (observation_subscribers_[i] != nullptr) { + observation_subscribers_[i]->subscribe(); + } + } + for (size_t i = 0; i < observation_buffers_.size(); ++i) { + if (observation_buffers_[i] != nullptr) { + observation_buffers_[i]->ResetLastUpdated(); + } + } +} + +void ObstacleLayer::Deactivate() { + for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) { + if (observation_subscribers_[i] != nullptr) + observation_subscribers_[i]->unsubscribe(); + } +} + +void ObstacleLayer::Reset() { + Deactivate(); + ResetMaps(); + is_current_ = true; + Activate(); +} + +bool ObstacleLayer::GetMarkingObservations(std::vector &marking_observations) const { + bool current = true; + // get the marking observations + for (size_t i = 0; i < marking_buffers_.size(); ++i) { + marking_buffers_[i]->Lock(); + marking_buffers_[i]->GetObservations(marking_observations); + current = marking_buffers_[i]->IsCurrent() && current; + marking_buffers_[i]->Unlock(); + } + marking_observations.insert(marking_observations.end(), + static_marking_observations_.begin(), static_marking_observations_.end()); + return current; +} + +bool ObstacleLayer::GetClearingObservations(std::vector &clearing_observations) const { + bool current = true; + // get the clearing observations + for (unsigned int i = 0; i < clearing_buffers_.size(); ++i) { + clearing_buffers_[i]->Lock(); + clearing_buffers_[i]->GetObservations(clearing_observations); + current = clearing_buffers_[i]->IsCurrent() && current; + clearing_buffers_[i]->Unlock(); + } + clearing_observations.insert(clearing_observations.end(), + static_clearing_observations_.begin(), static_clearing_observations_.end()); + return current; +} + +void ObstacleLayer::RaytraceFreespace(const Observation &clearing_observation, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { + double ox = clearing_observation.origin_.x; + double oy = clearing_observation.origin_.y; + pcl::PointCloud cloud = *(clearing_observation.cloud_); + + // get the map coordinates of the origin of the sensor + unsigned int x0, y0; + if (!World2Map(ox, oy, x0, y0)) { + return; + } + + // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later + double origin_x = origin_x_, origin_y = origin_y_; + double map_end_x = origin_x + size_x_ * resolution_; + double map_end_y = origin_y + size_y_ * resolution_; + + Touch(ox, oy, min_x, min_y, max_x, max_y); + + // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it + for (unsigned int i = 0; i < cloud.points.size(); ++i) { + double wx = cloud.points[i].x; + double wy = cloud.points[i].y; + + // now we also need to make sure that the enpoint we're raytracing + // to isn't off the map and scale if necessary + double a = wx - ox; + double b = wy - oy; + + // the minimum value to raytrace from is the origin + if (wx < origin_x) { + double t = (origin_x - ox) / a; + wx = origin_x; + wy = oy + b * t; + } + if (wy < origin_y) { + double t = (origin_y - oy) / b; + wx = ox + a * t; + wy = origin_y; + } + + // the maximum value to raytrace to is the end of the map + if (wx > map_end_x) { + double t = (map_end_x - ox) / a; + wx = map_end_x - .001; + wy = oy + b * t; + } + if (wy > map_end_y) { + double t = (map_end_y - oy) / b; + wx = ox + a * t; + wy = map_end_y - .001; + } + + // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint + unsigned int x1, y1; + + // check for legality just in case + if (!World2Map(wx, wy, x1, y1)) + continue; + + unsigned int cell_raytrace_range = World2Cell(clearing_observation.raytrace_range_); + MarkCell marker(costmap_, FREE_SPACE); + // and finally... we can execute our trace to clear obstacles along that line + RaytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); + + UpdateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); + } +} + +void ObstacleLayer::UpdateRaytraceBounds(double ox, + double oy, + double wx, + double wy, + double range, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { + double dx = wx - ox, dy = wy - oy; + double full_distance = hypot(dx, dy); + double scale = std::min(1.0, range / full_distance); + double ex = ox + dx * scale, ey = oy + dy * scale; + Touch(ex, ey, min_x, min_y, max_x, max_y); +} + +void ObstacleLayer::UpdateFootprint(double robot_x, + double robot_y, + double robot_yaw, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { + if (!footprint_clearing_enabled_) + return; + TransformFootprint(robot_x, robot_y, robot_yaw, GetFootprint(), transformed_footprint_); + + for (size_t i = 0; i < transformed_footprint_.size(); i++) { + Touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } +} + +} //namespace roborts_costmap \ No newline at end of file diff --git a/roborts_costmap/src/static_layer.cpp b/roborts_costmap/src/static_layer.cpp new file mode 100755 index 00000000..5f3ca10d --- /dev/null +++ b/roborts_costmap/src/static_layer.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ +#include "static_layer_setting.pb.h" +#include "static_layer.h" + +namespace roborts_costmap { + +void StaticLayer::OnInitialize() { + ros::NodeHandle nh; + is_current_ = true; + ParaStaticLayer para_static_layer; + + std::string static_map = ros::package::getPath("roborts_costmap") + \ + "/config/static_layer_config.prototxt"; + roborts_common::ReadProtoFromTextFile(static_map.c_str(), ¶_static_layer); + global_frame_ = layered_costmap_-> GetGlobalFrameID(); + first_map_only_ = para_static_layer.first_map_only(); + subscribe_to_updates_ = para_static_layer.subscribe_to_updates(); + track_unknown_space_ = para_static_layer.track_unknown_space(); + use_maximum_ = para_static_layer.use_maximum(); + int temp_threshold = para_static_layer.lethal_threshold(); + lethal_threshold_ = std::max(std::min(100, temp_threshold), 0); + trinary_costmap_ = para_static_layer.trinary_map(); + unknown_cost_value_ = para_static_layer.unknown_cost_value(); + map_received_ = false; + bool is_debug_ = para_static_layer.is_debug(); + map_topic_ = para_static_layer.topic_name(); + map_sub_ = nh.subscribe(map_topic_.c_str(), 1, &StaticLayer::InComingMap, this); + ros::Rate temp_rate(10); + while(!map_received_) { + ros::spinOnce(); + temp_rate.sleep(); + } + staic_layer_x_ = staic_layer_y_ = 0; + width_ = size_x_; + height_ = size_y_; + is_enabled_ = true; + has_updated_data_ = true; +} + +void StaticLayer::MatchSize() { + if (!layered_costmap_->IsRolling()) { + Costmap2D* master = layered_costmap_->GetCostMap(); + ResizeMap(master->GetSizeXCell(), master->GetSizeYCell(), master->GetResolution(), + master->GetOriginX(), master->GetOriginY()); + } +} + +void StaticLayer::InComingMap(const nav_msgs::OccupancyGridConstPtr &new_map) { + unsigned int temp_index = 0; + unsigned char value = 0; + unsigned int size_x = new_map->info.width, size_y = new_map->info.height; + auto resolution = new_map->info.resolution; + auto origin_x = new_map->info.origin.position.x; + auto origin_y = new_map->info.origin.position.y; + auto master_map = layered_costmap_->GetCostMap(); + if(!layered_costmap_->IsRolling() && (master_map->GetSizeXCell() != size_x || master_map->GetSizeYCell() != size_y || + master_map->GetResolution() != resolution || master_map->GetOriginX() != origin_x || master_map->GetOriginY() != origin_y || + !layered_costmap_->IsSizeLocked())) { + layered_costmap_->ResizeMap(size_x, size_y, resolution, origin_x, origin_y, true); + } else if(size_x_ != size_x || size_y_ != size_y || resolution_ != resolution || origin_x_ != origin_x || origin_y_ != origin_y) { + ResizeMap(size_x, size_y, resolution, origin_x, origin_y); + } + + for (auto i = 0; i < size_y; i++) { + for (auto j = 0; j < size_x; j++) { + value = new_map->data[temp_index]; + costmap_[temp_index] = InterpretValue(value); + ++temp_index; + } + } + map_received_ = true; + has_updated_data_ = true; + map_frame_ = new_map->header.frame_id; + staic_layer_x_ = staic_layer_y_ = 0; + width_ = size_x_; + height_ = size_y_; + if (first_map_only_) { + map_sub_.shutdown(); + } +} + +unsigned char StaticLayer::InterpretValue(unsigned char value) { + // check if the static value is above the unknown or lethal thresholds + if (track_unknown_space_ && value == unknown_cost_value_) + return NO_INFORMATION; + else if (!track_unknown_space_ && value == unknown_cost_value_) + return FREE_SPACE; + else if (value >= lethal_threshold_) + return LETHAL_OBSTACLE; + else if (trinary_costmap_) + return FREE_SPACE; + + double scale = (double) value / lethal_threshold_; + return scale * LETHAL_OBSTACLE; +} + +void StaticLayer::Activate() { + OnInitialize(); +} + +void StaticLayer::Deactivate() { +// delete cost_map_; + //shut down the map topic message subscriber + map_sub_.shutdown(); +} + +void StaticLayer::Reset() { + if(first_map_only_) { + has_updated_data_ = true; + } else { + OnInitialize(); + } +} + +void StaticLayer::UpdateBounds(double robot_x, + double robot_y, + double robot_yaw, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { + double wx, wy; + if(!layered_costmap_->IsRollingWindow()) { + if(!map_received_ || !(has_updated_data_ || has_extra_bounds_)) { + return; + } + } + //just make sure the value is normal + UseExtraBounds(min_x, min_y, max_x, max_y); + Map2World(staic_layer_x_, staic_layer_y_, wx, wy); + *min_x = std::min(wx, *min_x); + *min_y = std::min(wy, *min_y); + Map2World(staic_layer_x_+ width_, staic_layer_y_ + height_, wx, wy); + *max_x = std::max(*max_x, wx); + *max_y = std::max(*max_y, wy); + has_updated_data_ = false; +} + +void StaticLayer::UpdateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { + if(!map_received_) { + return; + } + if(!layered_costmap_->IsRollingWindow()) { + if(!use_maximum_) { + UpdateOverwriteByAll(master_grid, min_i, min_j, max_i, max_j); + } else { + UpdateOverwriteByMax(master_grid, min_i, min_j, max_i, max_j); + } + } else { + unsigned int mx, my; + double wx, wy; + tf::StampedTransform temp_transform; + try { + tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), temp_transform); + } + catch (tf::TransformException ex) { + ROS_ERROR("%s", ex.what()); + return; + } + for(auto i = min_i; i < max_i; ++i) { + for(auto j = min_j; j < max_j; ++j) { + layered_costmap_->GetCostMap()->Map2World(i, j, wx, wy); + tf::Point p(wx, wy, 0); + p = temp_transform(p); + if(World2Map(p.x(), p.y(), mx, my)){ + if(!use_maximum_) { + master_grid.SetCost(i, j, GetCost(mx, my)); + } + else { + master_grid.SetCost(i, j, std::max(master_grid.GetCost(i, j), GetCost(i, j))); + } + } + } + } + } +} + +} //namespace roborts_costmap + diff --git a/roborts_costmap/src/test_costmap.cpp b/roborts_costmap/src/test_costmap.cpp new file mode 100755 index 00000000..30d7abf0 --- /dev/null +++ b/roborts_costmap/src/test_costmap.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************/ + +#include +#include + +#include "costmap_interface.h" + +void SignalHandler(int signal){ + if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ + ros::shutdown(); + } +} + +int main(int argc, char** argv) { + + signal(SIGINT, SignalHandler); + signal(SIGTERM,SignalHandler); + ros::init(argc, argv, "test_costmap", ros::init_options::NoSigintHandler); + tf::TransformListener tf(ros::Duration(10)); + + std::string local_map = ros::package::getPath("roborts_costmap") + \ + "/config/costmap_parameter_config_for_local_plan.prototxt"; + roborts_costmap::CostmapInterface costmap_interface("map", + tf, + local_map.c_str()); + + ros::AsyncSpinner async_spinner(1); + async_spinner.start(); + ros::waitForShutdown(); + return 0; +} diff --git a/roborts_decision/CMakeLists.txt b/roborts_decision/CMakeLists.txt new file mode 100755 index 00000000..dacaada1 --- /dev/null +++ b/roborts_decision/CMakeLists.txt @@ -0,0 +1,56 @@ +project(roborts_decision) +cmake_minimum_required(VERSION 3.1) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) + +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + nav_msgs + geometry_msgs + actionlib + roborts_common + roborts_msgs + roborts_costmap + ) + +find_package(Eigen3 REQUIRED) +find_package(ProtoBuf REQUIRED) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +#generate proto files +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto + DecisionProtoSrc + DecisionProtoHds + ${ProtoFiles} + ) + +catkin_package() + +add_executable(behavior_test_node + ${DecisionProtoHds} + ${DecisionProtoSrc} + example_behavior/escape_behavior.h + example_behavior/back_boot_area_behavior.h + example_behavior/chase_behavior.h + example_behavior/patrol_behavior.h + example_behavior/search_behavior.h + behavior_test.cpp + executor/chassis_executor.cpp + ) + +target_link_libraries(behavior_test_node + PRIVATE + roborts_costmap + ${catkin_LIBRARIES} + ${PROTOBUF_LIBRARIES} + ) +add_dependencies(behavior_test_node + roborts_msgs_generate_messages) diff --git a/roborts_decision/behavior_test.cpp b/roborts_decision/behavior_test.cpp new file mode 100644 index 00000000..621ec822 --- /dev/null +++ b/roborts_decision/behavior_test.cpp @@ -0,0 +1,96 @@ +#include + +#include "executor/chassis_executor.h" + +#include "example_behavior/back_boot_area_behavior.h" +#include "example_behavior/escape_behavior.h" +#include "example_behavior/chase_behavior.h" +#include "example_behavior/search_behavior.h" +#include "example_behavior/patrol_behavior.h" +#include "example_behavior/goal_behavior.h" + +void Command(); +char command = '0'; + +int main(int argc, char **argv) { + ros::init(argc, argv, "behavior_test_node"); + std::string full_path = ros::package::getPath("roborts_decision") + "/config/decision.prototxt"; + + auto chassis_executor = new roborts_decision::ChassisExecutor; + auto blackboard = new roborts_decision::Blackboard(full_path); + + roborts_decision::BackBootAreaBehavior back_boot_area_behavior(chassis_executor, blackboard, full_path); + roborts_decision::ChaseBehavior chase_behavior(chassis_executor, blackboard, full_path); + roborts_decision::SearchBehavior search_behavior(chassis_executor, blackboard, full_path); + roborts_decision::EscapeBehavior escape_behavior(chassis_executor, blackboard, full_path); + roborts_decision::PatrolBehavior patrol_behavior(chassis_executor, blackboard, full_path); + roborts_decision::GoalBehavior goal_behavior(chassis_executor, blackboard); + + auto command_thread= std::thread(Command); + ros::Rate rate(10); + while(ros::ok()){ + ros::spinOnce(); + switch (command) { + //back to boot area + case '1': + back_boot_area_behavior.Run(); + break; + //patrol + case '2': + patrol_behavior.Run(); + break; + //chase. + case '3': + chase_behavior.Run(); + break; + //search + case '4': + search_behavior.Run(); + break; + //escape. + case '5': + escape_behavior.Run(); + break; + //goal. + case '6': + goal_behavior.Run(); + break; + case 27: + if (command_thread.joinable()){ + command_thread.join(); + } + return 0; + default: + break; + } + rate.sleep(); + } + + + return 0; +} + +void Command() { + + while (command != 27) { + std::cout << "**************************************************************************************" << std::endl; + std::cout << "*********************************please send a command********************************" << std::endl; + std::cout << "1: back boot area behavior" << std::endl + << "2: patrol behavior" << std::endl + << "3: chase_behavior" << std::endl + << "4: search behavior" << std::endl + << "5: escape behavior" << std::endl + << "6: goal behavior" << std::endl + << "esc: exit program" << std::endl; + std::cout << "**************************************************************************************" << std::endl; + std::cout << "> "; + std::cin >> command; + if (command != '1' && command != '2' && command != '3' && command != '4' && command != '5' && command != '6' && command != 27) { + std::cout << "please input again!" << std::endl; + std::cout << "> "; + std::cin >> command; + } + + } +} + diff --git a/roborts_decision/behavior_tree/behavior_node.h b/roborts_decision/behavior_tree/behavior_node.h new file mode 100755 index 00000000..a250c8dd --- /dev/null +++ b/roborts_decision/behavior_tree/behavior_node.h @@ -0,0 +1,787 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DECISION_BEHAVIOR_NODE_H +#define ROBORTS_DECISION_BEHAVIOR_NODE_H + +#include +#include +#include + +#include "../blackboard/blackboard.h" +#include "behavior_state.h" + +namespace roborts_decision{ +/** + * @brief Type of behavior tree node + */ +enum class BehaviorType { + PARALLEL, ///{ + public: + + typedef std::shared_ptr Ptr; + BehaviorNode(){} + /** + * @brief Constructor of BehaviorNode + * @param name Name of the behavior node + * @param behavior_type Type of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + */ + BehaviorNode(std::string name, BehaviorType behavior_type, const Blackboard::Ptr &blackboard_ptr): + name_(name), + behavior_type_(behavior_type), + blackboard_ptr_(blackboard_ptr), + behavior_state_(BehaviorState::IDLE), + level_(0){} + + virtual ~BehaviorNode()= default; + /** + * @brief Run the behavior node + * @details Roughly including 3 process + * 1. OnInitilaize: Initialize or reset some variables, when tick the node that is not running. + * 2. Update: Update and feedback the behavior state. + * 3. OnTerminate: Reset or recover after getting the result. + * @return Behavior state + */ + BehaviorState Run(){ + + if (behavior_state_ != BehaviorState::RUNNING) { + OnInitialize(); + } + + behavior_state_ = Update(); + + if (behavior_state_ != BehaviorState::RUNNING) { + OnTerminate(behavior_state_); + } + + return behavior_state_; + } + /** + * @brief Reset the behavior node + * @details manually invoke the IDLE terminate function when the node is in running status + */ + virtual void Reset(){ + if (behavior_state_ == BehaviorState::RUNNING){ + behavior_state_ = BehaviorState::IDLE; + OnTerminate(behavior_state_); + } + } + /** + * @brief Get the type of the behavior node + * @return The type of the behavior node + */ + BehaviorType GetBehaviorType(){ + return behavior_type_; + } + /** + * @brief Get the state of the behavior node + * @return The state of the behavior node + */ + BehaviorState GetBehaviorState(){ + return behavior_state_; + } + /** + * @brief Get the name of the behavior node + * @return The name of the behavior node + */ + std::string GetName(){ + return name_.c_str(); + } + /** + * @brief Set the parent of the behavior node + * @param parent_node_ptr + */ + void SetParent(BehaviorNode::Ptr parent_node_ptr){ + parent_node_ptr_ = parent_node_ptr; + } + /** + * @brief Get the parent node of the behavior node + * @return The parent node of the behavior node + */ + BehaviorNode::Ptr GetParent(){ + return parent_node_ptr_ ; + } + /** + * @brief Get the child node of the behavior node + * @return The child node of the behavior node, for the base class it always returns nullptr + */ + virtual BehaviorNode::Ptr GetChild(){ + return nullptr; + } + /** + * @brief Get the level of the behavior node + * @return The level of the behavior node + */ + unsigned int GetLevel(){ + return level_; + } + /** + * @brief Set the level of the behavior node + * @param level The expected level of the behavior node + */ + void SetLevel(unsigned int level){ + level_ = level; + } + protected: + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update() = 0; + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize() = 0; + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state) = 0; + + //! Node name + std::string name_; + //! State +// std::mutex behavior_state_mutex_; + BehaviorState behavior_state_; + //! Type + BehaviorType behavior_type_; + //! Blackboard + Blackboard::Ptr blackboard_ptr_; + //! Parent Node Pointer + BehaviorNode::Ptr parent_node_ptr_; + //! Level of the tree + unsigned int level_; +}; +/** + * @brief Behavior tree action node inherited from BehaviorNode + */ +class ActionNode: public BehaviorNode{ + public: + /** + * @brief Constructor of ActionNode + * @param name Name of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + */ + ActionNode(std::string name, const Blackboard::Ptr &blackboard_ptr): + BehaviorNode::BehaviorNode(name,BehaviorType::ACTION, blackboard_ptr){} + virtual ~ActionNode() = default; + + protected: + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize() = 0; + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update() = 0; + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state) = 0; + +}; +/** + * @brief Behavior tree decorator node inherited from BehaviorNode + */ +class DecoratorNode: public BehaviorNode{ + public: + /** + * @brief Constructor of DecoratorNode + * @param name Name of the behavior node + * @param behavior_type Type of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + */ + DecoratorNode(std::string name, BehaviorType behavior_type, const Blackboard::Ptr &blackboard_ptr): + BehaviorNode::BehaviorNode(name, behavior_type, blackboard_ptr), + child_node_ptr_(nullptr){ } + virtual ~DecoratorNode() = default; + /** + * @brief Get the child/decorated node of the behavior node + * @return the child/decorated node of the behavior node + */ + virtual BehaviorNode::Ptr GetChild(){ + return child_node_ptr_ ; + } + /** + * @brief Set the child/decorated node of the behavior node + * @param child_node_ptr the child/decorated node of the behavior node + */ + void SetChild(const BehaviorNode::Ptr &child_node_ptr) { + if(child_node_ptr_){ + child_node_ptr_->SetParent(nullptr); + child_node_ptr->SetLevel(0); + } + child_node_ptr_ = child_node_ptr; + child_node_ptr_->SetParent(shared_from_this()); + child_node_ptr_->SetLevel(level_); + } + + protected: + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize() = 0; + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update() = 0; + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state) = 0; + //! the child/decorated node of the behavior node + BehaviorNode::Ptr child_node_ptr_; +}; +/** + * @brief Behavior tree precondition node inherited from DecoratorNode + */ +class PreconditionNode: public DecoratorNode{ + public: + /** + * @brief Constructor of PreconditionNode + * @param name Name of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + * @param precondition_function the determined function of the precondition node + * @param abort_type Abort type of the precondition node + */ + PreconditionNode(std::string name, const Blackboard::Ptr &blackboard_ptr, + std::function precondition_function = std::function(), + AbortType abort_type = AbortType::NONE): + DecoratorNode::DecoratorNode(name, BehaviorType::PRECONDITION, blackboard_ptr), + precondition_function_(precondition_function), abort_type_(abort_type){} + virtual ~PreconditionNode() = default; + /** + * @brief Get the abort type of the precondition node + * @return the abort type of the precondition node + */ + AbortType GetAbortType(){ + return abort_type_; + } + + protected: + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize() { + ROS_INFO("%s %s", name_.c_str(), __FUNCTION__); + } + + virtual bool Precondition(){ + if(precondition_function_){ + return precondition_function_(); + } + else{ + ROS_ERROR("There is no chosen precondition function, then return false by default!"); + return false; + } + }; + /** + * @brief Tick the node and update the state of the behavior node + * @details Every tick cycle, precondition node will reevaluate the precondition and + * tick its decorated node according to the abort type + * @return the state of the behavior node + */ + virtual BehaviorState Update(){ + if(child_node_ptr_ == nullptr){ + return BehaviorState::SUCCESS; + } + // Reevaluation + if(Reevaluation()){ + BehaviorState state = child_node_ptr_->Run(); + return state; + } + return BehaviorState::FAILURE; + } + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state) { + switch (state){ + case BehaviorState::IDLE: + ROS_INFO("%s %s IDLE!", name_.c_str(), __FUNCTION__); + //TODO: the following recovery measure is called by parent node, and deliver to reset its running child node + child_node_ptr_->Reset(); + break; + case BehaviorState::SUCCESS: + ROS_INFO("%s %s SUCCESS!", name_.c_str(), __FUNCTION__); + break; + case BehaviorState::FAILURE: + ROS_INFO("%s %s FAILURE!", name_.c_str(), __FUNCTION__); + //TODO: the following recovery measure is in failure situation caused by precondition false. + child_node_ptr_->Reset(); + break; + default: + ROS_INFO("%s %s ERROR!", name_.c_str(), __FUNCTION__); + return; + } + } + /** + * @brief Reevaluate the precondition every tick according to abort type + * @return True if reevaluation passes + */ + virtual bool Reevaluation(); + //! the determined function of the precondition node + std::function precondition_function_; + //! the abort type of the precondition node + AbortType abort_type_; +}; +/** + * @brief Behavior tree composite node inherited from BehaviorNode + */ +class CompositeNode: public BehaviorNode{ + public: + /** + * @brief Constructor of CompositeNode + * @param name Name of the behavior node + * @param behavior_type Type of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + */ + CompositeNode(std::string name, BehaviorType behavior_type, const Blackboard::Ptr &blackboard_ptr): + BehaviorNode::BehaviorNode(name, behavior_type, blackboard_ptr), + children_node_index_(0) { + } + + virtual ~CompositeNode()= default; + /** + * @brief Add child behavior node to the composite node + * @param child_node_ptr The expected child behavior node + */ + virtual void AddChildren(const BehaviorNode::Ptr& child_node_ptr){ + children_node_ptr_.push_back(child_node_ptr); + child_node_ptr->SetParent(shared_from_this()); + child_node_ptr->SetLevel(level_+1); + } + /** + * @brief Add a list of child behavior nodes to the composite node + * @param child_node_ptr A list of the expected child behavior nodes + */ + virtual void AddChildren(std::initializer_list child_node_ptr_list){ + for (auto child_node_ptr = child_node_ptr_list.begin(); child_node_ptr!=child_node_ptr_list.end(); child_node_ptr++) { + children_node_ptr_.push_back(*child_node_ptr); + (*child_node_ptr)->SetParent(shared_from_this()); + (*child_node_ptr)->SetLevel(level_+1); + } + } + /** + * @brief Get the child behavior node that it is turn to tick + * @return The child behavior node + */ + virtual BehaviorNode::Ptr GetChild(){ + return children_node_ptr_.at(children_node_index_) ; + } + /** + * @brief Get the list of child behavior nodes + * @return The list of child behavior nodes + */ + std::vector& GetChildren(){ + return children_node_ptr_; + } + /** + * @brief Get the index of the child behavior node that it is turn to tick + * @return The index of the child behavior node that it is turn to tick + */ + unsigned int GetChildrenIndex(){ + return children_node_index_; + } + /** + * @brief Get the number of child behavior nodes + * @return The number of child behavior nodes + */ + unsigned int GetChildrenNum(){ + return children_node_ptr_.size(); + } + + protected: + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update() = 0; + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize() = 0; + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state) = 0; + //! the list of child nodes + std::vector children_node_ptr_; + //! the index of child nodes + unsigned int children_node_index_; + +}; +/** + * @brief Behavior tree selector node inherited from CompositeNode + */ +class SelectorNode: public CompositeNode{ + public: + /** + * @brief Constructor of SelectorNode + * @param name Name of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + */ + SelectorNode(std::string name, const Blackboard::Ptr &blackboard_ptr): + CompositeNode::CompositeNode(name, BehaviorType::SELECTOR, blackboard_ptr) { + } + virtual ~SelectorNode() = default; + /** + * @brief Overwrite the function in the CompositeNode to add the reevaluation features for selector node + * @param child_node_ptr The expected child behavior node + */ + virtual void AddChildren(const BehaviorNode::Ptr& child_node_ptr){ + + CompositeNode::AddChildren(child_node_ptr); + + children_node_reevaluation_.push_back + (child_node_ptr->GetBehaviorType()==BehaviorType::PRECONDITION + && (std::dynamic_pointer_cast(child_node_ptr)->GetAbortType() == AbortType::LOW_PRIORITY + ||std::dynamic_pointer_cast(child_node_ptr)->GetAbortType() == AbortType::BOTH)); + + } + /** + * @brief Overwrite the function in the CompositeNode to add the reevaluation features for selector node + * @param child_node_ptr A list of the expected child behavior nodes + */ + virtual void AddChildren(std::initializer_list child_node_ptr_list){ + + CompositeNode::AddChildren(child_node_ptr_list); + + for (auto child_node_ptr = child_node_ptr_list.begin(); child_node_ptr!=child_node_ptr_list.end(); child_node_ptr++) { + children_node_reevaluation_.push_back + ((*child_node_ptr)->GetBehaviorType()==BehaviorType::PRECONDITION + && (std::dynamic_pointer_cast(*child_node_ptr)->GetAbortType() == AbortType::LOW_PRIORITY + ||std::dynamic_pointer_cast(*child_node_ptr)->GetAbortType() == AbortType::BOTH)); + } + } + /** + * @brief Set the index of the child node to tick + * @param children_node_index The expected index of the child node to tick + */ + void SetChildrenIndex(unsigned int children_node_index){ + children_node_index_=children_node_index; + } + protected: + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize(){ + children_node_index_ = 0; + ROS_INFO("%s %s", name_.c_str(), __FUNCTION__); + } + + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update(){ + + if (children_node_ptr_.size() == 0) { + return BehaviorState::SUCCESS; + } + + //Reevaluation + for(unsigned int index = 0; index < children_node_index_; index++){ + ROS_INFO("Reevaluation"); + if (children_node_reevaluation_.at(index)){ + BehaviorState state = children_node_ptr_.at(index)->Run(); + if(index == children_node_index_){ + ROS_INFO("%s abort goes on! ", name_.c_str()); + if (state != BehaviorState::FAILURE) { + return state; + } + ++children_node_index_; + break; + } + } + } + + while(true){ + + BehaviorState state = children_node_ptr_.at(children_node_index_)->Run(); + + if (state != BehaviorState::FAILURE) { + return state; + } + + if (++children_node_index_ == children_node_ptr_.size()) { + children_node_index_ = 0; + return BehaviorState::FAILURE; + } + + } + } + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state){ + switch (state){ + case BehaviorState::IDLE: + ROS_INFO("%s %s IDLE!", name_.c_str(), __FUNCTION__); + //TODO: the following recovery measure is called by parent node, and deliver to reset its running child node + children_node_ptr_.at(children_node_index_)->Reset(); + break; + case BehaviorState::SUCCESS: + ROS_INFO("%s %s SUCCESS!", name_.c_str(), __FUNCTION__); + break; + case BehaviorState::FAILURE: + ROS_INFO("%s %s FAILURE!", name_.c_str(), __FUNCTION__); + break; + default: + ROS_INFO("%s %s ERROR!", name_.c_str(), __FUNCTION__); + return; + } + } + + //! the list of reeavalution state for each child node + std::vector children_node_reevaluation_; +}; +/** + * @brief Behavior tree sequence node inherited from CompositeNode + */ +class SequenceNode: public CompositeNode{ + public: + /** + * @brief Constructor of SequenceNode + * @param name Name of behavior node + * @param blackboard_ptr Blackboard of behavior node + */ + SequenceNode(std::string name, const Blackboard::Ptr &blackboard_ptr): + CompositeNode::CompositeNode(name, BehaviorType::SEQUENCE, blackboard_ptr) {} + virtual ~SequenceNode() = default; + + protected: + /** + * @brief Initialize something before starting to tick the node + */ + virtual void OnInitialize(){ + children_node_index_ = 0; + ROS_INFO("%s %s", name_.c_str(), __FUNCTION__); + } + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update(){ + + if (children_node_ptr_.size() == 0) { + return BehaviorState::SUCCESS; + } + + while(true){ + + BehaviorState state = children_node_ptr_.at(children_node_index_)->Run(); + + if (state != BehaviorState::SUCCESS) { + return state; + } + if (++children_node_index_ == children_node_ptr_.size()) { + children_node_index_ = 0; + return BehaviorState::SUCCESS; + } + + } + } + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state){ + switch (state){ + case BehaviorState::IDLE: + ROS_INFO("%s %s IDLE!", name_.c_str(), __FUNCTION__); + children_node_ptr_.at(children_node_index_)->Reset(); + break; + case BehaviorState::SUCCESS: + ROS_INFO("%s %s SUCCESS!", name_.c_str(), __FUNCTION__); + break; + case BehaviorState::FAILURE: + ROS_INFO("%s %s FAILURE!", name_.c_str(), __FUNCTION__); + break; + default: + ROS_INFO("%s %s ERROR!", name_.c_str(), __FUNCTION__); + return; + } + } +}; +/** + * @brief Behavior tree parallel node inherited from CompositeNode + */ +class ParallelNode: public CompositeNode{ + public: + /** + * @brief Constructor of ParallelNode + * @param name Name of the behavior node + * @param blackboard_ptr Blackboard of the behavior node + * @param threshold Threshold number of child nodes to determine behavior state of success + */ + ParallelNode(std::string name, const Blackboard::Ptr &blackboard_ptr, + unsigned int threshold): + CompositeNode::CompositeNode(name, BehaviorType::PARALLEL, blackboard_ptr), + threshold_(threshold), + success_count_(0), + failure_count_(0){} + + virtual ~ParallelNode() = default; + protected: + /** + * @brief Initialize something before starting to tick the node + * @details Initialize and reset the success and failure count of each child node + */ + virtual void OnInitialize(){ + failure_count_=0; + success_count_=0; + children_node_done_.clear(); + children_node_done_.resize(children_node_ptr_.size(),false); + ROS_INFO("%s %s", name_.c_str(), __FUNCTION__); + } + /** + * @brief Tick the node and update the state of the behavior node + * @return the state of the behavior node + */ + virtual BehaviorState Update(){ + + if (children_node_ptr_.size() == 0) { + return BehaviorState::SUCCESS; + } + + for (unsigned int index = 0; index!=children_node_ptr_.size(); index++) { + if (children_node_done_.at(index) == false){ + BehaviorState state = children_node_ptr_.at(index)->Run(); + + if (state == BehaviorState::SUCCESS) { + children_node_done_.at(index) = true; + if (++success_count_ >= threshold_) { + return BehaviorState::SUCCESS; + } + } + else if (state == BehaviorState::FAILURE) { + children_node_done_.at(index) = true; + if (++failure_count_ >= children_node_ptr_.size()-threshold_) { + return BehaviorState::FAILURE; + } + } + + } + } + return BehaviorState::RUNNING; + } + /** + * @brief Recover or reset something After getting the result + * @param state Input behavior state + */ + virtual void OnTerminate(BehaviorState state){ + switch (state){ + case BehaviorState::IDLE: + ROS_INFO("%s %s IDLE!", name_.c_str(), __FUNCTION__); + break; + case BehaviorState::SUCCESS: + ROS_INFO("%s %s SUCCESS!", name_.c_str(), __FUNCTION__); + break; + case BehaviorState::FAILURE: + ROS_INFO("%s %s FAILURE!", name_.c_str(), __FUNCTION__); + break; + default: + ROS_INFO("%s %s ERROR!", name_.c_str(), __FUNCTION__); + return; + } + //TODO: no matter what state, the node would reset all running children to terminate. + for (unsigned int index = 0; index!=children_node_ptr_.size(); index++) { + children_node_ptr_.at(index)->Reset(); + } + } + + //! a list of result checker flags for each child node + std::vector children_node_done_; + //! the current number of child nodes with success behavior state + unsigned int success_count_; + //! the current number of child nodes with failure behavior state + unsigned int failure_count_; + //! the number of child nodes with success behavior state to determine success of the parallel node + unsigned int threshold_; +}; + +bool PreconditionNode::Reevaluation(){ + + // Back Reevaluation + if (parent_node_ptr_ != nullptr && parent_node_ptr_->GetBehaviorType() == BehaviorType::SELECTOR + && (abort_type_ == AbortType::LOW_PRIORITY || abort_type_ == AbortType::BOTH)){ + auto parent_selector_node_ptr = std::dynamic_pointer_cast(parent_node_ptr_); + + auto parent_children = parent_selector_node_ptr->GetChildren(); + auto iter_in_parent = std::find(parent_children.begin(), parent_children.end(), shared_from_this()); + if (iter_in_parent == parent_children.end()) { + ROS_DEBUG("Can't find current node in parent!"); + return false; + } + unsigned int index_in_parent = iter_in_parent - parent_children.begin(); + if (index_in_parent < parent_selector_node_ptr->GetChildrenIndex()){ + if(Precondition()){ + //Abort Measures + ROS_INFO("Abort happens!"); + parent_children.at(parent_selector_node_ptr->GetChildrenIndex())->Reset(); + parent_selector_node_ptr->SetChildrenIndex(index_in_parent); + return true; + } + else{ + return false; + } + } + } + // Self Reevaluation + + if(abort_type_== AbortType::SELF || abort_type_== AbortType::BOTH + || child_node_ptr_->GetBehaviorState() != BehaviorState::RUNNING){ + if(!Precondition()){ + return false; + } + } + return true; +} + +} //namespace roborts_decision + + +#endif //ROBORTS_DECISION_BEHAVIOR_NODE_H diff --git a/roborts_decision/behavior_tree/behavior_state.h b/roborts_decision/behavior_tree/behavior_state.h new file mode 100644 index 00000000..6a44b22b --- /dev/null +++ b/roborts_decision/behavior_tree/behavior_state.h @@ -0,0 +1,18 @@ +// +// Created by autocar on 18-12-21. +// + +#ifndef ROBORTS_DECISION_BEHAVIOR_STATE_H +#define ROBORTS_DECISION_BEHAVIOR_STATE_H +namespace roborts_decision { +/** + * @brief Behavior state + */ +enum class BehaviorState { + RUNNING, ///< Running state in process + SUCCESS, ///< Success state as result + FAILURE, ///< Failure state as result + IDLE, ///< Idle state, state as default or after cancellation +}; +} +#endif //ROBORTS_DECISION_BEHAVIOR_STATE_H diff --git a/roborts_decision/behavior_tree/behavior_tree.h b/roborts_decision/behavior_tree/behavior_tree.h new file mode 100755 index 00000000..0d47c65b --- /dev/null +++ b/roborts_decision/behavior_tree/behavior_tree.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DECISION_BEHAVIOR_TREE_H +#define ROBORTS_DECISION_BEHAVIOR_TREE_H + +#include + +#include + +#include "behavior_node.h" + +namespace roborts_decision{ +/** + * @brief Behavior tree class to initialize and execute the whole tree + */ +class BehaviorTree { + public: + /** + * @brief Constructor of BehaviorTree + * @param root_node root node of the behavior tree + * @param cycle_duration tick duration of the behavior tree (unit ms) + */ + BehaviorTree(BehaviorNode::Ptr root_node, int cycle_duration) : + root_node_(root_node), + cycle_duration_(cycle_duration) {} + /** + * @brief Loop to tick the behavior tree + */ + void Run() { + + unsigned int frame = 0; + while (ros::ok() ) { + + std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); + // Update the blackboard data + ros::spinOnce(); + ROS_INFO("Frame : %d", frame); + root_node_->Run(); + + std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); + std::chrono::milliseconds execution_duration = + std::chrono::duration_cast(end_time - start_time); + std::chrono::milliseconds sleep_time = cycle_duration_ - execution_duration; + + if (sleep_time > std::chrono::microseconds(0)) { + + std::this_thread::sleep_for(sleep_time); + ROS_INFO("Excution Duration: %ld / %ld ms", cycle_duration_.count(), cycle_duration_.count()); + + } else { + + ROS_WARN("The time tick once is %ld beyond the expected time %ld", execution_duration.count(), cycle_duration_.count()); + + } + ROS_INFO("----------------------------------"); + frame++; + } + } + private: + //! root node of the behavior tree + BehaviorNode::Ptr root_node_; + //! tick duration of the behavior tree (unit ms) + std::chrono::milliseconds cycle_duration_; + +}; + +}//namespace roborts_decision + +#endif //ROBORTS_DECISION_BEHAVIOR_TREE_H diff --git a/roborts_decision/blackboard/blackboard.h b/roborts_decision/blackboard/blackboard.h new file mode 100755 index 00000000..cb7db78a --- /dev/null +++ b/roborts_decision/blackboard/blackboard.h @@ -0,0 +1,232 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_DECISION_BLACKBOARD_H +#define ROBORTS_DECISION_BLACKBOARD_H + +#include +#include +#include +#include +#include + +#include "roborts_msgs/ArmorDetectionAction.h" + +#include "io/io.h" +#include "../proto/decision.pb.h" +#include "costmap/costmap_interface.h" + +namespace roborts_decision{ + +class Blackboard { + public: + typedef std::shared_ptr Ptr; + typedef roborts_costmap::CostmapInterface CostMap; + typedef roborts_costmap::Costmap2D CostMap2D; + explicit Blackboard(const std::string &proto_file_path): + enemy_detected_(false), + armor_detection_actionlib_client_("armor_detection_node_action", true){ + + tf_ptr_ = std::make_shared(ros::Duration(10)); + + std::string map_path = ros::package::getPath("roborts_costmap") + \ + "/config/costmap_parameter_config_for_decision.prototxt"; + costmap_ptr_ = std::make_shared("decision_costmap", *tf_ptr_, + map_path); + charmap_ = costmap_ptr_->GetCostMap()->GetCharMap(); + + costmap_2d_ = costmap_ptr_->GetLayeredCostmap()->GetCostMap(); + + // Enemy fake pose + ros::NodeHandle rviz_nh("/move_base_simple"); + enemy_sub_ = rviz_nh.subscribe("goal", 1, &Blackboard::GoalCallback, this); + + ros::NodeHandle nh; + + roborts_decision::DecisionConfig decision_config; + roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config); + + if (!decision_config.simulate()){ + + armor_detection_actionlib_client_.waitForServer(); + + ROS_INFO("Armor detection module has been connected!"); + + armor_detection_goal_.command = 1; + armor_detection_actionlib_client_.sendGoal(armor_detection_goal_, + actionlib::SimpleActionClient::SimpleDoneCallback(), + actionlib::SimpleActionClient::SimpleActiveCallback(), + boost::bind(&Blackboard::ArmorDetectionFeedbackCallback, this, _1)); + } + + + } + + ~Blackboard() = default; + + + // Enemy + void ArmorDetectionFeedbackCallback(const roborts_msgs::ArmorDetectionFeedbackConstPtr& feedback){ + if (feedback->detected){ + enemy_detected_ = true; + ROS_INFO("Find Enemy!"); + + tf::Stamped tf_pose, global_tf_pose; + geometry_msgs::PoseStamped camera_pose_msg, global_pose_msg; + camera_pose_msg = feedback->enemy_pos; + + double distance = std::sqrt(camera_pose_msg.pose.position.x * camera_pose_msg.pose.position.x + + camera_pose_msg.pose.position.y * camera_pose_msg.pose.position.y); + double yaw = atan(camera_pose_msg.pose.position.y / camera_pose_msg.pose.position.x); + + //camera_pose_msg.pose.position.z=camera_pose_msg.pose.position.z; + tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, + 0, + yaw); + camera_pose_msg.pose.orientation.w = quaternion.w(); + camera_pose_msg.pose.orientation.x = quaternion.x(); + camera_pose_msg.pose.orientation.y = quaternion.y(); + camera_pose_msg.pose.orientation.z = quaternion.z(); + poseStampedMsgToTF(camera_pose_msg, tf_pose); + + tf_pose.stamp_ = ros::Time(0); + try + { + tf_ptr_->transformPose("map", tf_pose, global_tf_pose); + tf::poseStampedTFToMsg(global_tf_pose, global_pose_msg); + + if(GetDistance(global_pose_msg, enemy_pose_)>0.2 || GetAngle(global_pose_msg, enemy_pose_) > 0.2){ + enemy_pose_ = global_pose_msg; + + } + } + catch (tf::TransformException &ex) { + ROS_ERROR("tf error when transform enemy pose from camera to map"); + } + } else{ + enemy_detected_ = false; + } + + } + + geometry_msgs::PoseStamped GetEnemy() const { + return enemy_pose_; + } + + bool IsEnemyDetected() const{ + ROS_INFO("%s: %d", __FUNCTION__, (int)enemy_detected_); + return enemy_detected_; + } + + // Goal + void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal){ + new_goal_ = true; + goal_ = *goal; + } + + geometry_msgs::PoseStamped GetGoal() const { + return goal_; + } + + bool IsNewGoal(){ + if(new_goal_){ + new_goal_ = false; + return true; + } else{ + return false; + } + } + /*---------------------------------- Tools ------------------------------------------*/ + + double GetDistance(const geometry_msgs::PoseStamped &pose1, + const geometry_msgs::PoseStamped &pose2) { + const geometry_msgs::Point point1 = pose1.pose.position; + const geometry_msgs::Point point2 = pose2.pose.position; + const double dx = point1.x - point2.x; + const double dy = point1.y - point2.y; + return std::sqrt(dx * dx + dy * dy); + } + + double GetAngle(const geometry_msgs::PoseStamped &pose1, + const geometry_msgs::PoseStamped &pose2) { + const geometry_msgs::Quaternion quaternion1 = pose1.pose.orientation; + const geometry_msgs::Quaternion quaternion2 = pose2.pose.orientation; + tf::Quaternion rot1, rot2; + tf::quaternionMsgToTF(quaternion1, rot1); + tf::quaternionMsgToTF(quaternion2, rot2); + return rot1.angleShortestPath(rot2); + } + + const geometry_msgs::PoseStamped GetRobotMapPose() { + UpdateRobotPose(); + return robot_map_pose_; + } + + const std::shared_ptr GetCostMap(){ + return costmap_ptr_; + } + + const CostMap2D* GetCostMap2D() { + return costmap_2d_; + } + + const unsigned char* GetCharMap() { + return charmap_; + } + + private: + void UpdateRobotPose() { + tf::Stamped robot_tf_pose; + robot_tf_pose.setIdentity(); + + robot_tf_pose.frame_id_ = "base_link"; + robot_tf_pose.stamp_ = ros::Time(); + try { + geometry_msgs::PoseStamped robot_pose; + tf::poseStampedTFToMsg(robot_tf_pose, robot_pose); + tf_ptr_->transformPose("map", robot_pose, robot_map_pose_); + } + catch (tf::LookupException &ex) { + ROS_ERROR("Transform Error looking up robot pose: %s", ex.what()); + } + } + //! tf + std::shared_ptr tf_ptr_; + + //! Enenmy detection + ros::Subscriber enemy_sub_; + + //! Goal info + geometry_msgs::PoseStamped goal_; + bool new_goal_; + + //! Enemy info + actionlib::SimpleActionClient armor_detection_actionlib_client_; + roborts_msgs::ArmorDetectionGoal armor_detection_goal_; + geometry_msgs::PoseStamped enemy_pose_; + bool enemy_detected_; + + //! cost map + std::shared_ptr costmap_ptr_; + CostMap2D* costmap_2d_; + unsigned char* charmap_; + + //! robot map pose + geometry_msgs::PoseStamped robot_map_pose_; + +}; +} //namespace roborts_decision +#endif //ROBORTS_DECISION_BLACKBOARD_H diff --git a/roborts_decision/cmake_module/FindEigen3.cmake b/roborts_decision/cmake_module/FindEigen3.cmake new file mode 100755 index 00000000..239c1bbe --- /dev/null +++ b/roborts_decision/cmake_module/FindEigen3.cmake @@ -0,0 +1,86 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/roborts_decision/cmake_module/FindProtoBuf.cmake b/roborts_decision/cmake_module/FindProtoBuf.cmake new file mode 100755 index 00000000..3462e7be --- /dev/null +++ b/roborts_decision/cmake_module/FindProtoBuf.cmake @@ -0,0 +1,84 @@ +# Finds Google Protocol Buffers library and compilers and extends +# the standard cmake script + +find_package(Protobuf REQUIRED) +list(APPEND RRTS_INCLUDE_DIRS PUBLIC ${PROTOBUF_INCLUDE_DIR}) +list(APPEND RRTS_LINKER_LIBS PUBLIC ${PROTOBUF_LIBRARIES}) + +# As of Ubuntu 14.04 protoc is no longer a part of libprotobuf-dev package +# and should be installed separately as in: sudo apt-get install protobuf-compiler +if(EXISTS ${PROTOBUF_PROTOC_EXECUTABLE}) + message(STATUS "Found PROTOBUF Compiler: ${PROTOBUF_PROTOC_EXECUTABLE}") +else() + message(FATAL_ERROR "Could not find PROTOBUF Compiler") +endif() + +if (PROTOBUF_FOUND) + message ("protobuf ${PROTOBUF_LIBRARIES} found") +else () + message (FATAL_ERROR "Cannot find protobuf") +endif () + +# place where to generate protobuf sources +#set(proto_gen_folder "${PROJECT_BINARY_DIR}/include/caffe/proto") +#include_directories("${PROJECT_BINARY_DIR}/include") + +set(PROTOBUF_GENERATE_CPP_APPEND_PATH TRUE) + +################################################################################################ +# Modification of standard 'protobuf_generate_cpp()' with output dir parameter +# Usage: +# caffe_protobuf_generate_cpp_py( ) +function(rrts_protobuf_generate_cpp output_dir proto_srcs proto_hdrs) + if(NOT ARGN) + message(SEND_ERROR "Error: rrts_protobuf_generate_cpp() called without any proto files") + return() + endif() + + if(PROTOBUF_GENERATE_CPP_APPEND_PATH) + # Create an include path for each file specified + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(abs_path ${abs_fil} PATH) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + else() + set(_protoc_include -I ${CMAKE_CURRENT_SOURCE_DIR}) + endif() + + if(DEFINED PROTOBUF_IMPORT_DIRS) + foreach(dir ${PROTOBUF_IMPORT_DIRS}) + get_filename_component(abs_path ${dir} ABSOLUTE) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + endif() + + set(${proto_srcs}) + set(${proto_hdrs}) + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(fil_we ${fil} NAME_WE) + + list(APPEND ${proto_srcs} "${output_dir}/${fil_we}.pb.cc") + list(APPEND ${proto_hdrs} "${output_dir}/${fil_we}.pb.h") + + add_custom_command( + OUTPUT "${output_dir}/${fil_we}.pb.cpp" + "${output_dir}/${fil_we}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${output_dir} ${_protoc_include} ${abs_fil} + DEPENDS ${abs_fil} + COMMENT "Running C++ protocol buffer compiler on ${fil}" + VERBATIM ) + endforeach() + + set_source_files_properties(${${proto_srcs}} ${${proto_hdrs}} PROPERTIES GENERATED TRUE) + set(${proto_srcs} ${${proto_srcs}} PARENT_SCOPE) + set(${proto_hdrs} ${${proto_hdrs}} PARENT_SCOPE) +endfunction() diff --git a/roborts_decision/config/decision.prototxt b/roborts_decision/config/decision.prototxt new file mode 100644 index 00000000..ae32fd59 --- /dev/null +++ b/roborts_decision/config/decision.prototxt @@ -0,0 +1,267 @@ +simulate: true +master: true + +wing_bot { + start_position { + x: 1.25 # 7.23 + y: 1.17 # 4.32 + z: 0 + + roll: 0 + pitch: 0 + yaw: 1.57 # -1.57 + } +} + +master_bot { + start_position { + x: 0.56 # 7.68 + y: 0.56 # 4.77 + z: 0 + + roll: 0 + pitch: 0 + yaw: 0 # 3.14 + } +} + +# icra2019 map +point { + x: 6.14 + y: 4.45 + z: 0 + + roll: 0 + pitch: 0 + yaw: 0 +} + +point { + x: 6.73 + y: 0.82 + z: 0 + + roll: 0 + pitch: 0 + yaw: 3.14 +} + + +point { + x: 2.49 + y: 4.64 + z: 0 + + roll: 0 + pitch: 0 + yaw: 3.14 +} + +point { + x: 1.26 + y: 1.02 + z: 0 + + roll: 0 + pitch: 0 + yaw: 0 +} + +escape { + left_x_limit: 2.5 + right_x_limit: 5.5 + robot_x_limit: 4 + left_random_min_x: 0 + left_random_max_x: 2.5 + right_random_min_x: 5.5 + right_random_max_x: 8 +} + +buff_point { + + x: 4.15 + y: 2.85 + z: 0 + + roll: 0 + pitch: 0 + yaw: -0.52 #2.62 +} + +buff_point { + + x: 4.35 + y: 2.65 + z: 0 + + roll: 0 + pitch: 0 + yaw: -0.52 +} + +buff_point { + + x: 4.15 + y: 2.85 + z: 0 + + roll: 0 + pitch: 0 + yaw: -0.52 +} + +buff_point { + + x: 4.35 + y: 2.65 + z: 0 + + roll: 0 + pitch: 0 + yaw: -0.52 +} + +search_limit { + x_limit: 4.2 + y_limit: 2.75 +} + +whirl_vel { + angle_x_vel: 0 + angle_y_vel: 0 + angle_z_vel: 1 +} +wing_bot_task_point { + x: 4.2 + y: 0.87 + z: 0 + + roll: 0 + pitch: 0 + yaw: 1.57 +} + +search_region_1 { + x: 2.86 + y: 2.50 + z: 0 + + roll: 0 + pitch: 0 + yaw: -1.57 +} + +search_region_1 { + x: 1.78 + y: 0.94 + z: 0 + + roll: 0 + pitch: 0 + yaw: 3.14 +} + +search_region_1 { + x: 1.4 + y: 2.18 + z: 0 + + roll: 0 + pitch: 0 + yaw: 1.57 +} + +search_region_2 { + x: 5.35 + y: 0.8 + z: 0 + + roll: 0 + pitch: 0 + yaw: 0 +} + +search_region_2 { + x: 7.40 + y: 2.10 + z: 0 + + roll: 0 + pitch: 0 + yaw: 1.57 +} + +search_region_2 { + x: 5.35 + y: 2.24 + z: 0 + + roll: 0 + pitch: 0 + yaw: 3.14 +} + +search_region_3 { + x: 3.05 + y: 4.56 + z: 0 + + roll: 0 + pitch: 0 + yaw: 3.14 +} + +search_region_3 { + x: 0.8 + y: 4.02 + z: 0 + + roll: 0 + pitch: 0 + yaw: -1.57 +} + +search_region_3 { + x: 2.84 + y: 3.47 + z: 0 + + roll: 0 + pitch: 0 + yaw: 0 +} + +search_region_4 { + x: 5.55 + y: 2.93 + z: 0 + + roll: 0 + pitch: 0 + yaw: 1.57 +} + +search_region_4 { + x: 6.14 + y: 4.34 + z: 0 + + roll: 0 + pitch: 0 + yaw: 0 +} + +search_region_4 { + x: 6.83 + y: 3.05 + z: 0 + + roll: 0 + pitch: 0 + yaw: -1.57 +} + + + + + diff --git a/roborts_decision/example_behavior/back_boot_area_behavior.h b/roborts_decision/example_behavior/back_boot_area_behavior.h new file mode 100644 index 00000000..a121b5ee --- /dev/null +++ b/roborts_decision/example_behavior/back_boot_area_behavior.h @@ -0,0 +1,110 @@ +#ifndef ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H +#define ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H + +#include "io/io.h" + +#include "../blackboard/blackboard.h" +#include "../executor/chassis_executor.h" +#include "../behavior_tree/behavior_state.h" +#include "../proto/decision.pb.h" + +#include "line_iterator.h" + +namespace roborts_decision { +class BackBootAreaBehavior { + public: + BackBootAreaBehavior(ChassisExecutor* &chassis_executor, + Blackboard* &blackboard, + const std::string & proto_file_path) : chassis_executor_(chassis_executor), + blackboard_(blackboard) { + + + boot_position_.header.frame_id = "map"; + boot_position_.pose.orientation.x = 0; + boot_position_.pose.orientation.y = 0; + boot_position_.pose.orientation.z = 0; + boot_position_.pose.orientation.w = 1; + + boot_position_.pose.position.x = 0; + boot_position_.pose.position.y = 0; + boot_position_.pose.position.z = 0; + + if (!LoadParam(proto_file_path)) { + ROS_ERROR("%s can't open file", __FUNCTION__); + } + + } + + void Run() { + + auto executor_state = Update(); + + if (executor_state != BehaviorState::RUNNING) { + auto robot_map_pose = blackboard_->GetRobotMapPose(); + auto dx = boot_position_.pose.position.x - robot_map_pose.pose.position.x; + auto dy = boot_position_.pose.position.y - robot_map_pose.pose.position.y; + + auto boot_yaw = tf::getYaw(boot_position_.pose.orientation); + auto robot_yaw = tf::getYaw(robot_map_pose.pose.orientation); + + tf::Quaternion rot1, rot2; + tf::quaternionMsgToTF(boot_position_.pose.orientation, rot1); + tf::quaternionMsgToTF(robot_map_pose.pose.orientation, rot2); + auto d_yaw = rot1.angleShortestPath(rot2); + + if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) > 0.2 || d_yaw > 0.5) { + chassis_executor_->Execute(boot_position_); + + } + } + } + + void Cancel() { + chassis_executor_->Cancel(); + } + + BehaviorState Update() { + return chassis_executor_->Update(); + } + + bool LoadParam(const std::string &proto_file_path) { + roborts_decision::DecisionConfig decision_config; + if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) { + return false; + } + + boot_position_.header.frame_id = "map"; + + boot_position_.pose.position.x = decision_config.master_bot().start_position().x(); + boot_position_.pose.position.z = decision_config.master_bot().start_position().z(); + boot_position_.pose.position.y = decision_config.master_bot().start_position().y(); + + auto master_quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.master_bot().start_position().roll(), + decision_config.master_bot().start_position().pitch(), + decision_config.master_bot().start_position().yaw()); + boot_position_.pose.orientation = master_quaternion; + + return true; + } + + ~BackBootAreaBehavior() = default; + + private: + //! executor + ChassisExecutor* const chassis_executor_; + + //! perception information + Blackboard* const blackboard_; + + //! boot position + geometry_msgs::PoseStamped boot_position_; + + //! chase buffer + std::vector chase_buffer_; + unsigned int chase_count_; + +}; +} + + +#endif //ROBORTS_DECISION_BACK_BOOT_AREA_BEHAVIOR_H diff --git a/roborts_decision/example_behavior/chase_behavior.h b/roborts_decision/example_behavior/chase_behavior.h new file mode 100644 index 00000000..a939435e --- /dev/null +++ b/roborts_decision/example_behavior/chase_behavior.h @@ -0,0 +1,171 @@ +#ifndef ROBORTS_DECISION_CHASE_BEHAVIOR_H +#define ROBORTS_DECISION_CHASE_BEHAVIOR_H + +#include "io/io.h" + +#include "../blackboard/blackboard.h" +#include "../executor/chassis_executor.h" +#include "../behavior_tree/behavior_state.h" +#include "../proto/decision.pb.h" + +#include "line_iterator.h" + +namespace roborts_decision { +class ChaseBehavior { + public: + ChaseBehavior(ChassisExecutor* &chassis_executor, + Blackboard* &blackboard, + const std::string & proto_file_path) : chassis_executor_(chassis_executor), + blackboard_(blackboard) { + + + chase_goal_.header.frame_id = "map"; + chase_goal_.pose.orientation.x = 0; + chase_goal_.pose.orientation.y = 0; + chase_goal_.pose.orientation.z = 0; + chase_goal_.pose.orientation.w = 1; + + chase_goal_.pose.position.x = 0; + chase_goal_.pose.position.y = 0; + chase_goal_.pose.position.z = 0; + + chase_buffer_.resize(2); + chase_count_ = 0; + + cancel_goal_ = true; + } + + void Run() { + + auto executor_state = Update(); + + auto robot_map_pose = blackboard_->GetRobotMapPose(); + if (executor_state != BehaviorState::RUNNING) { + + chase_buffer_[chase_count_++ % 2] = blackboard_->GetEnemy(); + + chase_count_ = chase_count_ % 2; + + auto dx = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.x - robot_map_pose.pose.position.x; + auto dy = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.y - robot_map_pose.pose.position.y; + auto yaw = std::atan2(dy, dx); + + if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) >= 1.0 && std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <= 2.0) { + if (cancel_goal_) { + chassis_executor_->Cancel(); + cancel_goal_ = false; + } + return; + + } else { + + auto orientation = tf::createQuaternionMsgFromYaw(yaw); + geometry_msgs::PoseStamped reduce_goal; + reduce_goal.pose.orientation = robot_map_pose.pose.orientation; + + reduce_goal.header.frame_id = "map"; + reduce_goal.header.stamp = ros::Time::now(); + reduce_goal.pose.position.x = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.x - 1.2 * cos(yaw); + reduce_goal.pose.position.y = chase_buffer_[(chase_count_ + 2 - 1) % 2].pose.position.y - 1.2 * sin(yaw); + auto enemy_x = reduce_goal.pose.position.x; + auto enemy_y = reduce_goal.pose.position.y; + reduce_goal.pose.position.z = 1; + unsigned int goal_cell_x, goal_cell_y; + + // if necessary add mutex lock + //blackboard_->GetCostMap2D()->GetMutex()->lock(); + auto get_enemy_cell = blackboard_->GetCostMap2D()->World2Map(enemy_x, + enemy_y, + goal_cell_x, + goal_cell_y); + //blackboard_->GetCostMap2D()->GetMutex()->unlock(); + + if (!get_enemy_cell) { + return; + } + + auto robot_x = robot_map_pose.pose.position.x; + auto robot_y = robot_map_pose.pose.position.y; + unsigned int robot_cell_x, robot_cell_y; + double goal_x, goal_y; + blackboard_->GetCostMap2D()->World2Map(robot_x, + robot_y, + robot_cell_x, + robot_cell_y); + + if (blackboard_->GetCostMap2D()->GetCost(goal_cell_x, goal_cell_y) >= 253) { + + bool find_goal = false; + for(FastLineIterator line( goal_cell_x, goal_cell_y, robot_cell_x, robot_cell_x); line.IsValid(); line.Advance()) { + + auto point_cost = blackboard_->GetCostMap2D()->GetCost((unsigned int) (line.GetX()), (unsigned int) (line.GetY())); //current point's cost + + if(point_cost >= 253){ + continue; + + } else { + find_goal = true; + blackboard_->GetCostMap2D()->Map2World((unsigned int) (line.GetX()), + (unsigned int) (line.GetY()), + goal_x, + goal_y); + + reduce_goal.pose.position.x = goal_x; + reduce_goal.pose.position.y = goal_y; + break; + } + + } + if (find_goal) { + cancel_goal_ = true; + chassis_executor_->Execute(reduce_goal); + } else { + if (cancel_goal_) { + chassis_executor_->Cancel(); + cancel_goal_ = false; + } + return; + } + + } else { + cancel_goal_ = true; + chassis_executor_->Execute(reduce_goal); + } + } + } + } + + void Cancel() { + chassis_executor_->Cancel(); + } + + BehaviorState Update() { + return chassis_executor_->Update(); + } + + void SetGoal(geometry_msgs::PoseStamped chase_goal) { + chase_goal_ = chase_goal; + } + + ~ChaseBehavior() = default; + + private: + //! executor + ChassisExecutor* const chassis_executor_; + + //! perception information + Blackboard* const blackboard_; + + //! chase goal + geometry_msgs::PoseStamped chase_goal_; + + //! chase buffer + std::vector chase_buffer_; + unsigned int chase_count_; + + //! cancel flag + bool cancel_goal_; +}; +} + +#endif //ROBORTS_DECISION_CHASE_BEHAVIOR_H diff --git a/roborts_decision/example_behavior/escape_behavior.h b/roborts_decision/example_behavior/escape_behavior.h new file mode 100644 index 00000000..43b344aa --- /dev/null +++ b/roborts_decision/example_behavior/escape_behavior.h @@ -0,0 +1,196 @@ +#ifndef ROBORTS_DECISION_ESCAPEBEHAVIOR_H +#define ROBORTS_DECISION_ESCAPEBEHAVIOR_H + +#include "io/io.h" +#include "roborts_msgs/TwistAccel.h" + +#include "../blackboard/blackboard.h" +#include "../executor/chassis_executor.h" +#include "../behavior_tree/behavior_state.h" +#include "../proto/decision.pb.h" + +#include "line_iterator.h" + +namespace roborts_decision{ +class EscapeBehavior { + public: + EscapeBehavior(ChassisExecutor* &chassis_executor, + Blackboard* &blackboard, + const std::string & proto_file_path) : chassis_executor_(chassis_executor), + blackboard_(blackboard) { + + // init whirl velocity + whirl_vel_.accel.linear.x = 0; + whirl_vel_.accel.linear.x = 0; + whirl_vel_.accel.linear.y = 0; + whirl_vel_.accel.linear.z = 0; + + whirl_vel_.accel.angular.x = 0; + whirl_vel_.accel.angular.y = 0; + whirl_vel_.accel.angular.z = 0; + + if (!LoadParam(proto_file_path)) { + ROS_ERROR("%s can't open file", __FUNCTION__); + } + + } + + void Run() { + + auto executor_state = Update(); + + if (executor_state != BehaviorState::RUNNING) { + + if (blackboard_->IsEnemyDetected()) { + + geometry_msgs::PoseStamped enemy; + enemy = blackboard_->GetEnemy(); + float goal_yaw, goal_x, goal_y; + unsigned int goal_cell_x, goal_cell_y; + unsigned int enemy_cell_x, enemy_cell_y; + + std::random_device rd; + std::mt19937 gen(rd()); + + auto robot_map_pose = blackboard_->GetRobotMapPose(); + float x_min, x_max; + if (enemy.pose.position.x < left_x_limit_) { + x_min = right_random_min_x_; + x_max = right_random_max_x_; + + } else if (enemy.pose.position.x > right_x_limit_) { + x_min = left_random_min_x_; + x_max = left_random_max_x_; + + } else { + if ((robot_x_limit_ - robot_map_pose.pose.position.x) >= 0) { + x_min = left_random_min_x_; + x_max = left_random_max_x_; + + } else { + x_min = right_random_min_x_; + x_max = right_random_max_x_; + } + } + + std::uniform_real_distribution x_uni_dis(x_min, x_max); + std::uniform_real_distribution y_uni_dis(0, 5); + //std::uniform_real_distribution yaw_uni_dis(-M_PI, M_PI); + + auto get_enemy_cell = blackboard_->GetCostMap2D()->World2Map(enemy.pose.position.x, + enemy.pose.position.y, + enemy_cell_x, + enemy_cell_y); + + if (!get_enemy_cell) { + chassis_executor_->Execute(whirl_vel_); + return; + } + + + while (true) { + goal_x = x_uni_dis(gen); + goal_y = y_uni_dis(gen); + auto get_goal_cell = blackboard_->GetCostMap2D()->World2Map(goal_x, + goal_y, + goal_cell_x, + goal_cell_y); + + if (!get_goal_cell) { + continue; + } + + auto index = blackboard_->GetCostMap2D()->GetIndex(goal_cell_x, goal_cell_y); +// costmap_2d_->GetCost(goal_cell_x, goal_cell_y); + if (blackboard_->GetCharMap()[index] >= 253) { + continue; + } + + unsigned int obstacle_count = 0; + for(FastLineIterator line( goal_cell_x, goal_cell_y, enemy_cell_x, enemy_cell_y); line.IsValid(); line.Advance()) { + auto point_cost = blackboard_->GetCostMap2D()->GetCost((unsigned int)(line.GetX()), (unsigned int)(line.GetY())); //current point's cost + + if(point_cost > 253){ + obstacle_count++; + } + + } + + if (obstacle_count > 5) { //TODO: this should write in the proto file + break; + } + } + Eigen::Vector2d pose_to_enemy(enemy.pose.position.x - robot_map_pose.pose.position.x, + enemy.pose.position.y - robot_map_pose.pose.position.y); + goal_yaw = static_cast (std::atan2(pose_to_enemy.coeffRef(1), pose_to_enemy.coeffRef(0))); + auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,goal_yaw); + + geometry_msgs::PoseStamped escape_goal; + escape_goal.header.frame_id = "map"; + escape_goal.header.stamp = ros::Time::now(); + escape_goal.pose.position.x = goal_x; + escape_goal.pose.position.y = goal_y; + escape_goal.pose.orientation = quaternion; + //return exploration_goal; + chassis_executor_->Execute(escape_goal); + } else { + chassis_executor_->Execute(whirl_vel_); + return; + } + } + + } + + void Cancel() { + chassis_executor_->Cancel(); + } + + BehaviorState Update() { + return chassis_executor_->Update(); + } + + bool LoadParam(const std::string &proto_file_path) { + roborts_decision::DecisionConfig decision_config; + if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) { + return false; + } + + left_x_limit_ = decision_config.escape().left_x_limit(); + right_x_limit_ = decision_config.escape().right_x_limit(); + robot_x_limit_ = decision_config.escape().robot_x_limit(); + left_random_min_x_ = decision_config.escape().left_random_min_x(); + left_random_max_x_ = decision_config.escape().left_random_max_x(); + right_random_min_x_ = decision_config.escape().right_random_min_x(); + right_random_max_x_ = decision_config.escape().right_random_max_x(); + + whirl_vel_.twist.angular.z = decision_config.whirl_vel().angle_z_vel(); + whirl_vel_.twist.angular.y = decision_config.whirl_vel().angle_y_vel(); + whirl_vel_.twist.angular.x = decision_config.whirl_vel().angle_x_vel(); + + return true; + } + + ~EscapeBehavior() { + + } + + private: + //! executor + ChassisExecutor* const chassis_executor_; + + float left_x_limit_, right_x_limit_; + float robot_x_limit_; + float left_random_min_x_, left_random_max_x_; + float right_random_min_x_, right_random_max_x_; + + //! perception information + Blackboard* const blackboard_; + + //! whirl velocity +// geometry_msgs::Twist whirl_vel_; + roborts_msgs::TwistAccel whirl_vel_; + +}; +} + +#endif //ROBORTS_DECISION_ESCAPEBEHAVIOR_H diff --git a/roborts_decision/example_behavior/goal_behavior.h b/roborts_decision/example_behavior/goal_behavior.h new file mode 100644 index 00000000..3dc9a5ca --- /dev/null +++ b/roborts_decision/example_behavior/goal_behavior.h @@ -0,0 +1,49 @@ +#ifndef ROBORTS_DECISION_GOAL_BEHAVIOR_H +#define ROBORTS_DECISION_GOAL_BEHAVIOR_H + + +#include "io/io.h" + +#include "../blackboard/blackboard.h" +#include "../executor/chassis_executor.h" +#include "../behavior_tree/behavior_state.h" + + +namespace roborts_decision { +class GoalBehavior { + public: + GoalBehavior(ChassisExecutor* &chassis_executor, + Blackboard* &blackboard) : + chassis_executor_(chassis_executor), + blackboard_(blackboard) { } + + void Run() { + if(blackboard_->IsNewGoal()){ + chassis_executor_->Execute(blackboard_->GetGoal()); + } + } + + void Cancel() { + chassis_executor_->Cancel(); + } + + BehaviorState Update() { + return chassis_executor_->Update(); + } + + ~GoalBehavior() = default; + + private: + //! executor + ChassisExecutor* const chassis_executor_; + + //! perception information + Blackboard* const blackboard_; + + //! planning goal + geometry_msgs::PoseStamped planning_goal_; + +}; +} + +#endif //ROBORTS_DECISION_GOAL_BEHAVIOR_H diff --git a/roborts_decision/example_behavior/line_iterator.h b/roborts_decision/example_behavior/line_iterator.h new file mode 100644 index 00000000..db5d8664 --- /dev/null +++ b/roborts_decision/example_behavior/line_iterator.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DECISION_LINE_ITERATOR_H +#define ROBORTS_DECISION_LINE_ITERATOR_H + +#include + +namespace roborts_decision { + +class FastLineIterator { + // this method is a modified version of base_local_planner/line_iterator.h + public: + FastLineIterator( int x0, int y0, int x1, int y1 ) + : x0_( x0 ) + , y0_( y0 ) + , x1_( x1 ) + , y1_( y1 ) + , x_( x0 ) + , y_( y0 ) + , deltax_(abs(x1 - x0)) + , deltay_(abs(y1 - y0)) + , curpixel_( 0 ) { + + xinc1_ = (x1 - x0) >0 ?1:-1; + xinc2_ = (x1 - x0) >0 ?1:-1; + + yinc1_ = (y1 - y0) >0 ?1:-1; + yinc2_ = (y1 - y0) >0 ?1:-1; + + if( deltax_ >= deltay_ ) { + xinc1_ = 0; + yinc2_ = 0; + den_ = 2 * deltax_; + num_ = deltax_; + numadd_ = 2 * deltay_; + numpixels_ = deltax_; + } else { + xinc2_ = 0; + yinc1_ = 0; + den_ = 2 * deltay_; + num_ = deltay_; + numadd_ = 2 * deltax_; + numpixels_ = deltay_; + } + } + ~FastLineIterator() = default; + bool IsValid() const { + return curpixel_ <= numpixels_; + } + + void Advance() { + num_ += numadd_; + if( num_ >= den_ ) { + num_ -= den_; + x_ += xinc1_; + y_ += yinc1_; + } + x_ += xinc2_; + y_ += yinc2_; + + curpixel_++; + } + + int GetX() const { return x_; } + int GetY() const { return y_; } + + int GetX0() const { return x0_; } + int GetY0() const { return y0_; } + + int GetX1() const { return x1_; } + int GetY1() const { return y1_; } + + private: + int x0_; + int y0_; + int x1_; + int y1_; + + int x_; + int y_; + + int deltax_; + int deltay_; + + int curpixel_; + + int xinc1_, xinc2_, yinc1_, yinc2_; + int den_, num_, numadd_, numpixels_; +}; + +} // namespace roborts_decision + +#endif //ROBORTS_DECISION_LINE_ITERATOR_H + diff --git a/roborts_decision/example_behavior/patrol_behavior.h b/roborts_decision/example_behavior/patrol_behavior.h new file mode 100644 index 00000000..3efb9719 --- /dev/null +++ b/roborts_decision/example_behavior/patrol_behavior.h @@ -0,0 +1,101 @@ +#ifndef ROBORTS_DECISION_PATROL_BEHAVIOR_H +#define ROBORTS_DECISION_PATROL_BEHAVIOR_H + +#include "io/io.h" + +#include "../blackboard/blackboard.h" +#include "../executor/chassis_executor.h" +#include "../behavior_tree/behavior_state.h" +#include "../proto/decision.pb.h" + +#include "line_iterator.h" + +namespace roborts_decision { +class PatrolBehavior { + public: + PatrolBehavior(ChassisExecutor* &chassis_executor, + Blackboard* &blackboard, + const std::string & proto_file_path) : chassis_executor_(chassis_executor), + blackboard_(blackboard) { + + patrol_count_ = 0; + point_size_ = 0; + + if (!LoadParam(proto_file_path)) { + ROS_ERROR("%s can't open file", __FUNCTION__); + } + + } + + void Run() { + + auto executor_state = Update(); + + std::cout << "state: " << (int)(executor_state) << std::endl; + + if (executor_state != BehaviorState::RUNNING) { + + if (patrol_goals_.empty()) { + ROS_ERROR("patrol goal is empty"); + return; + } + + std::cout << "send goal" << std::endl; + chassis_executor_->Execute(patrol_goals_[patrol_count_]); + patrol_count_ = ++patrol_count_ % point_size_; + + } + } + + void Cancel() { + chassis_executor_->Cancel(); + } + + BehaviorState Update() { + return chassis_executor_->Update(); + } + + bool LoadParam(const std::string &proto_file_path) { + roborts_decision::DecisionConfig decision_config; + if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) { + return false; + } + + point_size_ = (unsigned int)(decision_config.point().size()); + patrol_goals_.resize(point_size_); + for (int i = 0; i != point_size_; i++) { + patrol_goals_[i].header.frame_id = "map"; + patrol_goals_[i].pose.position.x = decision_config.point(i).x(); + patrol_goals_[i].pose.position.y = decision_config.point(i).y(); + patrol_goals_[i].pose.position.z = decision_config.point(i).z(); + + tf::Quaternion quaternion = tf::createQuaternionFromRPY(decision_config.point(i).roll(), + decision_config.point(i).pitch(), + decision_config.point(i).yaw()); + patrol_goals_[i].pose.orientation.x = quaternion.x(); + patrol_goals_[i].pose.orientation.y = quaternion.y(); + patrol_goals_[i].pose.orientation.z = quaternion.z(); + patrol_goals_[i].pose.orientation.w = quaternion.w(); + } + + return true; + } + + ~PatrolBehavior() = default; + + private: + //! executor + ChassisExecutor* const chassis_executor_; + + //! perception information + Blackboard* const blackboard_; + + //! patrol buffer + std::vector patrol_goals_; + unsigned int patrol_count_; + unsigned int point_size_; + +}; +} + +#endif //ROBORTS_DECISION_PATROL_BEHAVIOR_H diff --git a/roborts_decision/example_behavior/search_behavior.h b/roborts_decision/example_behavior/search_behavior.h new file mode 100644 index 00000000..413e8515 --- /dev/null +++ b/roborts_decision/example_behavior/search_behavior.h @@ -0,0 +1,208 @@ +#ifndef ROBORTS_DECISION_SEARCH_BEHAVIOR_H +#define ROBORTS_DECISION_SEARCH_BEHAVIOR_H + +#include "io/io.h" + +#include "../blackboard/blackboard.h" +#include "../executor/chassis_executor.h" +#include "../behavior_tree/behavior_state.h" +#include "../proto/decision.pb.h" + +#include "line_iterator.h" + +namespace roborts_decision { +class SearchBehavior { + public: + SearchBehavior(ChassisExecutor* &chassis_executor, + Blackboard* &blackboard, + const std::string & proto_file_path) : chassis_executor_(chassis_executor), + blackboard_(blackboard) { + + + last_position_.header.frame_id = "map"; + last_position_.pose.orientation.x = 0; + last_position_.pose.orientation.y = 0; + last_position_.pose.orientation.z = 0; + last_position_.pose.orientation.w = 1; + + last_position_.pose.position.x = 0; + last_position_.pose.position.y = 0; + last_position_.pose.position.z = 0; + + search_index_ = 0; + search_count_ = 0; + + if (!LoadParam(proto_file_path)) { + ROS_ERROR("%s can't open file", __FUNCTION__); + } + + } + + void Run() { + + auto executor_state = Update(); + + double yaw; + double x_diff; + double y_diff; + + if (executor_state != BehaviorState::RUNNING) { + auto robot_map_pose = blackboard_->GetRobotMapPose(); + if (search_count_ == 5) { + x_diff = last_position_.pose.position.x - robot_map_pose.pose.position.x; + y_diff = last_position_.pose.position.y - robot_map_pose.pose.position.y; + + auto last_x = last_position_.pose.position.x; + auto last_y = last_position_.pose.position.y; + + if (last_x < 4.2 && last_y < 2.75) { + search_region_ = search_region_1_; + + } else if (last_x > 4.2 && last_y < 2.75) { + search_region_ = search_region_2_; + + } else if (last_x < 4.2 && last_y > 2.75) { + search_region_ = search_region_3_; + + } else { + search_region_ = search_region_4_; + + } + + double search_min_dist = 99999; + for (unsigned int i = 0; i < search_region_.size(); ++i) { + auto dist_sq = std::pow(search_region_[i].pose.position.x - last_x, 2) + + std::pow(search_region_[i].pose.position.y - last_y, 2); + + if (dist_sq < search_min_dist) { + search_min_dist = dist_sq; + search_index_ = i; + } + } + + yaw = std::atan2(y_diff, x_diff); + + auto orientation = tf::createQuaternionMsgFromYaw(yaw); + + geometry_msgs::PoseStamped goal; + goal.header.frame_id = "map"; + goal.header.stamp = ros::Time::now(); + goal.pose.position = last_position_.pose.position; + goal.pose.orientation = orientation; + chassis_executor_->Execute(goal); + search_count_--; + + } else if (search_count_ > 0) { + auto search_goal = search_region_[(search_index_++ )]; + chassis_executor_->Execute(search_goal); + search_index_ = (unsigned int) (search_index_% search_region_.size()); + search_count_--; + + } + } + } + + void Cancel() { + chassis_executor_->Cancel(); + } + + BehaviorState Update() { + return chassis_executor_->Update(); + } + + bool LoadParam(const std::string &proto_file_path) { + roborts_decision::DecisionConfig decision_config; + if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) { + return false; + } + + // may have more efficient way to search a region(enemy where disappear) + search_region_.resize((unsigned int)(decision_config.search_region_1().size())); + for (int i = 0; i != decision_config.search_region_1().size(); i++) { + geometry_msgs::PoseStamped search_point; + search_point.header.frame_id = "map"; + search_point.pose.position.x = decision_config.search_region_1(i).x(); + search_point.pose.position.y = decision_config.search_region_1(i).y(); + search_point.pose.position.z = decision_config.search_region_1(i).z(); + + auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_1(i).roll(), + decision_config.search_region_1(i).pitch(), + decision_config.search_region_1(i).yaw()); + search_point.pose.orientation = quaternion; + search_region_1_.push_back(search_point); + } + + for (int i = 0; i != decision_config.search_region_2().size(); i++) { + geometry_msgs::PoseStamped search_point; + search_point.header.frame_id = "map"; + search_point.pose.position.x = decision_config.search_region_2(i).x(); + search_point.pose.position.y = decision_config.search_region_2(i).y(); + search_point.pose.position.z = decision_config.search_region_2(i).z(); + + auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_2(i).roll(), + decision_config.search_region_2(i).pitch(), + decision_config.search_region_2(i).yaw()); + search_point.pose.orientation = quaternion; + search_region_2_.push_back(search_point); + } + + for (int i = 0; i != decision_config.search_region_3().size(); i++) { + geometry_msgs::PoseStamped search_point; + search_point.header.frame_id = "map"; + search_point.pose.position.x = decision_config.search_region_3(i).x(); + search_point.pose.position.y = decision_config.search_region_3(i).y(); + search_point.pose.position.z = decision_config.search_region_3(i).z(); + + auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_3(i).roll(), + decision_config.search_region_3(i).pitch(), + decision_config.search_region_3(i).yaw()); + search_point.pose.orientation = quaternion; + search_region_3_.push_back(search_point); + } + + for (int i = 0; i != decision_config.search_region_4().size(); i++) { + geometry_msgs::PoseStamped search_point; + search_point.header.frame_id = "map"; + search_point.pose.position.x = decision_config.search_region_4(i).x(); + search_point.pose.position.y = decision_config.search_region_4(i).y(); + search_point.pose.position.z = decision_config.search_region_4(i).z(); + + auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_region_4(i).roll(), + decision_config.search_region_4(i).pitch(), + decision_config.search_region_4(i).yaw()); + search_point.pose.orientation = quaternion; + search_region_4_.push_back(search_point); + } + return true; + } + + void SetLastPosition(geometry_msgs::PoseStamped last_position) { + last_position_ = last_position; + search_count_ = 5; + } + + ~SearchBehavior() = default; + + private: + //! executor + ChassisExecutor* const chassis_executor_; + + //! perception information + Blackboard* const blackboard_; + + //! chase goal + geometry_msgs::PoseStamped last_position_; + + //! search buffer + std::vector search_region_1_; + std::vector search_region_2_; + std::vector search_region_3_; + std::vector search_region_4_; + std::vector search_region_; + unsigned int search_count_; + unsigned int search_index_; + +}; +} + +#endif //ROBORTS_DECISION_SEARCH_BEHAVIOR_H diff --git a/roborts_decision/executor/chassis_executor.cpp b/roborts_decision/executor/chassis_executor.cpp new file mode 100644 index 00000000..443bc876 --- /dev/null +++ b/roborts_decision/executor/chassis_executor.cpp @@ -0,0 +1,126 @@ +#include +#include "chassis_executor.h" + +namespace roborts_decision{ + +ChassisExecutor::ChassisExecutor():execution_mode_(ExcutionMode::IDLE_MODE), execution_state_(BehaviorState::IDLE), + global_planner_client_("global_planner_node_action", true), + local_planner_client_("local_planner_node_action", true) +{ + ros::NodeHandle nh; + cmd_vel_acc_pub_ = nh.advertise("cmd_vel_acc", 100); + cmd_vel_pub_ = nh.advertise("cmd_vel", 1); + global_planner_client_.waitForServer(); + ROS_INFO("Global planer server start!"); + local_planner_client_.waitForServer(); + ROS_INFO("Local planer server start!"); +} + +void ChassisExecutor::Execute(const geometry_msgs::PoseStamped &goal){ + execution_mode_ = ExcutionMode::GOAL_MODE; + global_planner_goal_.goal = goal; + global_planner_client_.sendGoal(global_planner_goal_, + GlobalActionClient::SimpleDoneCallback(), + GlobalActionClient::SimpleActiveCallback(), + boost::bind(&ChassisExecutor::GlobalPlannerFeedbackCallback, this, _1)); +} + +void ChassisExecutor::Execute(const geometry_msgs::Twist &twist){ + if( execution_mode_ == ExcutionMode::GOAL_MODE){ + Cancel(); + } + execution_mode_ = ExcutionMode::SPEED_MODE; + cmd_vel_pub_.publish(twist); +} + +void ChassisExecutor::Execute(const roborts_msgs::TwistAccel &twist_accel){ + if( execution_mode_ == ExcutionMode::GOAL_MODE){ + Cancel(); + } + execution_mode_ = ExcutionMode::SPEED_WITH_ACCEL_MODE; + + cmd_vel_acc_pub_.publish(twist_accel); +} + +BehaviorState ChassisExecutor::Update(){ + actionlib::SimpleClientGoalState state = actionlib::SimpleClientGoalState::LOST; + switch (execution_mode_){ + case ExcutionMode::IDLE_MODE: + execution_state_ = BehaviorState::IDLE; + break; + + case ExcutionMode::GOAL_MODE: + state = global_planner_client_.getState(); + if (state == actionlib::SimpleClientGoalState::ACTIVE){ + ROS_INFO("%s : ACTIVE", __FUNCTION__); + execution_state_ = BehaviorState::RUNNING; + + } else if (state == actionlib::SimpleClientGoalState::PENDING) { + ROS_INFO("%s : PENDING", __FUNCTION__); + execution_state_ = BehaviorState::RUNNING; + + } else if (state == actionlib::SimpleClientGoalState::SUCCEEDED) { + ROS_INFO("%s : SUCCEEDED", __FUNCTION__); + execution_state_ = BehaviorState::SUCCESS; + + } else if (state == actionlib::SimpleClientGoalState::ABORTED) { + ROS_INFO("%s : ABORTED", __FUNCTION__); + execution_state_ = BehaviorState::FAILURE; + + } else { + ROS_ERROR("Error: %s", state.toString().c_str()); + execution_state_ = BehaviorState::FAILURE; + } + break; + + case ExcutionMode::SPEED_MODE: + execution_state_ = BehaviorState::RUNNING; + break; + + case ExcutionMode::SPEED_WITH_ACCEL_MODE: + execution_state_ = BehaviorState::RUNNING; + break; + + default: + ROS_ERROR("Wrong Execution Mode"); + } + return execution_state_; + +}; + +void ChassisExecutor::Cancel(){ + switch (execution_mode_){ + case ExcutionMode::IDLE_MODE: + ROS_WARN("Nothing to be canceled."); + break; + + case ExcutionMode::GOAL_MODE: + global_planner_client_.cancelGoal(); + local_planner_client_.cancelGoal(); + execution_mode_ = ExcutionMode::IDLE_MODE; + break; + + case ExcutionMode::SPEED_MODE: + cmd_vel_pub_.publish(zero_twist_); + execution_mode_ = ExcutionMode::IDLE_MODE; + break; + + case ExcutionMode::SPEED_WITH_ACCEL_MODE: + cmd_vel_acc_pub_.publish(zero_twist_accel_); + execution_mode_ = ExcutionMode::IDLE_MODE; + usleep(50000); + break; + default: + ROS_ERROR("Wrong Execution Mode"); + } + +} + +void ChassisExecutor::GlobalPlannerFeedbackCallback(const roborts_msgs::GlobalPlannerFeedbackConstPtr& global_planner_feedback){ + if (!global_planner_feedback->path.poses.empty()) { + local_planner_goal_.route = global_planner_feedback->path; + local_planner_client_.sendGoal(local_planner_goal_); + } +} + +} \ No newline at end of file diff --git a/roborts_decision/executor/chassis_executor.h b/roborts_decision/executor/chassis_executor.h new file mode 100644 index 00000000..bd320604 --- /dev/null +++ b/roborts_decision/executor/chassis_executor.h @@ -0,0 +1,96 @@ +#ifndef ROBORTS_DECISION_CHASSIS_EXECUTOR_H +#define ROBORTS_DECISION_CHASSIS_EXECUTOR_H +#include +#include + +#include "roborts_msgs/GlobalPlannerAction.h" +#include "roborts_msgs/LocalPlannerAction.h" +#include "roborts_msgs/TwistAccel.h" +#include "geometry_msgs/Twist.h" + +#include "../behavior_tree/behavior_state.h" + +namespace roborts_decision{ +/*** + * @brief Chassis Executor to execute different abstracted task for chassis module + */ +class ChassisExecutor{ + + typedef actionlib::SimpleActionClient GlobalActionClient; + typedef actionlib::SimpleActionClient LocalActionClient; + public: + /** + * @brief Chassis execution mode for different tasks + */ + enum class ExcutionMode{ + IDLE_MODE, ///< Default idle mode with no task + GOAL_MODE, ///< Goal-targeted task mode using global and local planner + SPEED_MODE, ///< Velocity task mode + SPEED_WITH_ACCEL_MODE ///< Velocity with acceleration task mode + }; + /** + * @brief Constructor of ChassisExecutor + */ + ChassisExecutor(); + ~ChassisExecutor() = default; + /** + * @brief Execute the goal-targeted task using global and local planner with actionlib + * @param goal Given taget goal + */ + void Execute(const geometry_msgs::PoseStamped &goal); + /** + * @brief Execute the velocity task with publisher + * @param twist Given velocity + */ + void Execute(const geometry_msgs::Twist &twist); + /** + * @brief Execute the velocity with acceleration task with publisher + * @param twist_accel Given velocity with acceleration + */ + void Execute(const roborts_msgs::TwistAccel &twist_accel); + /** + * @brief Update the current chassis executor state + * @return Current chassis executor state(same with behavior state) + */ + BehaviorState Update(); + /** + * @brief Cancel the current task and deal with the mode transition + */ + void Cancel(); + + private: + /*** + * @brief Global planner actionlib feedback callback function to send the global planner path to local planner + * @param global_planner_feedback Global planner actionlib feedback, which mainly consists of global planner path output + */ + void GlobalPlannerFeedbackCallback(const roborts_msgs::GlobalPlannerFeedbackConstPtr& global_planner_feedback); + //! execution mode of the executor + ExcutionMode execution_mode_; + //! execution state of the executor (same with behavior state) + BehaviorState execution_state_; + + //! global planner actionlib client + actionlib::SimpleActionClient global_planner_client_; + //! local planner actionlib client + actionlib::SimpleActionClient local_planner_client_; + //! global planner actionlib goal + roborts_msgs::GlobalPlannerGoal global_planner_goal_; + //! local planner actionlib goal + roborts_msgs::LocalPlannerGoal local_planner_goal_; + + //! velocity control publisher in ROS + ros::Publisher cmd_vel_pub_; + //! zero twist in form of ROS geometry_msgs::Twist + geometry_msgs::Twist zero_twist_; + + //! velocity with accel publisher in ROS + ros::Publisher cmd_vel_acc_pub_; + //! zero twist with acceleration in form of ROS roborts_msgs::TwistAccel + roborts_msgs::TwistAccel zero_twist_accel_; + + +}; +} + + +#endif //ROBORTS_DECISION_CHASSIS_EXECUTOR_H \ No newline at end of file diff --git a/roborts_decision/executor/gimbal_executor.cpp b/roborts_decision/executor/gimbal_executor.cpp new file mode 100644 index 00000000..8671d55b --- /dev/null +++ b/roborts_decision/executor/gimbal_executor.cpp @@ -0,0 +1,62 @@ +#include "gimbal_executor.h" +namespace roborts_decision{ +GimbalExecutor::GimbalExecutor():excution_mode_(ExcutionMode::IDLE_MODE), + execution_state_(BehaviorState::IDLE){ + ros::NodeHandle nh; + cmd_gimbal_angle_pub_ = nh.advertise("cmd_gimbal_angle", 1); + cmd_gimbal_rate_pub_ = nh.advertise("cmd_gimbal_rate", 1); + +} + +void GimbalExecutor::Execute(const roborts_msgs::GimbalAngle &gimbal_angle){ + excution_mode_ = ExcutionMode::ANGLE_MODE; + cmd_gimbal_angle_pub_.publish(gimbal_angle); +} + +void GimbalExecutor::Execute(const roborts_msgs::GimbalRate &gimbal_rate){ + excution_mode_ = ExcutionMode::RATE_MODE; + cmd_gimbal_rate_pub_.publish(gimbal_rate); +} + +BehaviorState GimbalExecutor::Update(){ + switch (excution_mode_){ + case ExcutionMode::IDLE_MODE: + execution_state_ = BehaviorState::IDLE; + break; + + case ExcutionMode::ANGLE_MODE: + execution_state_ = BehaviorState::RUNNING; + break; + + case ExcutionMode::RATE_MODE: + execution_state_ = BehaviorState::RUNNING; + break; + + default: + ROS_ERROR("Wrong Execution Mode"); + } + return execution_state_; +} + +void GimbalExecutor::Cancel(){ + switch (excution_mode_){ + case ExcutionMode::IDLE_MODE: + ROS_WARN("Nothing to be canceled."); + break; + + case ExcutionMode::ANGLE_MODE: + cmd_gimbal_rate_pub_.publish(zero_gimbal_rate_); + excution_mode_ = ExcutionMode::IDLE_MODE; + break; + + case ExcutionMode::RATE_MODE: + cmd_gimbal_rate_pub_.publish(zero_gimbal_rate_); + excution_mode_ = ExcutionMode::IDLE_MODE; + break; + + default: + ROS_ERROR("Wrong Execution Mode"); + } + +} +} \ No newline at end of file diff --git a/roborts_decision/executor/gimbal_executor.h b/roborts_decision/executor/gimbal_executor.h new file mode 100644 index 00000000..d72dde36 --- /dev/null +++ b/roborts_decision/executor/gimbal_executor.h @@ -0,0 +1,67 @@ +#ifndef ROBORTS_DECISION_GIMBAL_EXECUTOR_H +#define ROBORTS_DECISION_GIMBAL_EXECUTOR_H +#include "ros/ros.h" + +#include "roborts_msgs/GimbalAngle.h" +#include "roborts_msgs/GimbalRate.h" + +#include "../behavior_tree/behavior_state.h" +namespace roborts_decision{ +/*** + * @brief Gimbal Executor to execute different abstracted task for gimbal module + */ +class GimbalExecutor{ + public: + /** + * @brief Gimbal execution mode for different tasks + */ + enum class ExcutionMode{ + IDLE_MODE, ///< Default idle mode with no task + ANGLE_MODE, ///< Angle task mode + RATE_MODE ///< Rate task mode + }; + /** + * @brief Constructor of GimbalExecutor + */ + GimbalExecutor(); + ~GimbalExecutor() = default; + /*** + * @brief Execute the gimbal angle task with publisher + * @param gimbal_angle Given gimbal angle + */ + void Execute(const roborts_msgs::GimbalAngle &gimbal_angle); + /*** + * @brief Execute the gimbal rate task with publisher + * @param gimbal_rate Given gimbal rate + */ + void Execute(const roborts_msgs::GimbalRate &gimbal_rate); + /** + * @brief Update the current gimbal executor state + * @return Current gimbal executor state(same with behavior state) + */ + BehaviorState Update(); + /** + * @brief Cancel the current task and deal with the mode transition + */ + void Cancel(); + + private: + //! execution mode of the executor + ExcutionMode excution_mode_; + //! execution state of the executor (same with behavior state) + BehaviorState execution_state_; + + //! gimbal rate control publisher in ROS + ros::Publisher cmd_gimbal_rate_pub_; + //! zero gimbal rate in form of ROS roborts_msgs::GimbalRate + roborts_msgs::GimbalRate zero_gimbal_rate_; + + //! gimbal angle control publisher in ROS + ros::Publisher cmd_gimbal_angle_pub_; + + +}; +} + + +#endif //ROBORTS_DECISION_GIMBAL_EXECUTOR_H \ No newline at end of file diff --git a/roborts_decision/package.xml b/roborts_decision/package.xml new file mode 100755 index 00000000..46ac45a8 --- /dev/null +++ b/roborts_decision/package.xml @@ -0,0 +1,31 @@ + + roborts_decision + 1.0.0 + + The roborts decision package provides decision-making for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + roscpp + roborts_msgs + rospy + roborts_common + roborts_costmap + + actionlib + roscpp + roborts_msgs + rospy + roborts_common + roborts_costmap + + + + + diff --git a/roborts_decision/proto/decision.proto b/roborts_decision/proto/decision.proto new file mode 100755 index 00000000..ea610bf2 --- /dev/null +++ b/roborts_decision/proto/decision.proto @@ -0,0 +1,55 @@ +syntax = "proto2"; +package roborts_decision; + +message Point { + optional float x = 1; + optional float y = 2; + optional float z = 3; + + optional float roll = 4; + optional float pitch = 5; + optional float yaw = 6; +} + +message EscapeConstraint { + optional float left_x_limit = 1; + optional float right_x_limit = 2; + optional float robot_x_limit = 3; + optional float left_random_min_x = 4; + optional float left_random_max_x = 5; + optional float right_random_min_x = 6; + optional float right_random_max_x = 7; +} + +message SearchConstraint { + optional float x_limit = 1; + optional float y_limit = 2; +} + +message WhirlVel { + optional float angle_x_vel = 1; + optional float angle_y_vel = 2; + optional float angle_z_vel = 3; +} + +message MultiRobot { + optional Point start_position = 7; +} + +message DecisionConfig { + repeated Point point = 1; + optional bool simulate = 2 [default = false]; + optional bool master = 3 [default = false]; + optional EscapeConstraint escape = 4; + repeated Point buff_point = 5; + optional SearchConstraint search_limit = 6; + optional WhirlVel whirl_vel = 7; + optional MultiRobot wing_bot = 8; + optional MultiRobot master_bot = 9; + optional Point wing_bot_task_point = 10; + repeated Point search_region_1 = 11; + repeated Point search_region_2 = 12; + repeated Point search_region_3 = 13; + repeated Point search_region_4 = 14; +} + diff --git a/roborts_detection/CMakeLists.txt b/roborts_detection/CMakeLists.txt new file mode 100644 index 00000000..e284d04d --- /dev/null +++ b/roborts_detection/CMakeLists.txt @@ -0,0 +1,30 @@ +project(roborts_detection) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) +set(COMPILE_PROTO 0) + +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + actionlib + cv_bridge + image_transport + std_msgs + sensor_msgs + roborts_msgs + roborts_common + ) + +find_package(ProtoBuf REQUIRED) +find_package(OpenCV 3 REQUIRED) +find_package(Eigen3 REQUIRED) + +catkin_package() + +include_directories(${EIGEN3_INCLUDE_DIR} util) + +add_subdirectory(util) +add_subdirectory(armor_detection) +#add_subdirectory(color_detection) diff --git a/roborts_detection/armor_detection/CMakeLists.txt b/roborts_detection/armor_detection/CMakeLists.txt new file mode 100755 index 00000000..90c8f7b4 --- /dev/null +++ b/roborts_detection/armor_detection/CMakeLists.txt @@ -0,0 +1,56 @@ +project(armor_detection) + +add_subdirectory(constraint_set) + + +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto + ArmorDetectionProtoSrc + ArmorDetectionProtoHds + ${ProtoFiles}) + + +#armor detection node +add_executable(${PROJECT_NAME}_node + ${ArmorDetectionProtoSrc} + ${ArmorDetectionProtoHds} + armor_detection_node.cpp + gimbal_control.cpp +) + +target_link_libraries(armor_detection_node + PRIVATE + detection::tool + detection::constraint_set + ${PROTOBUF_LIBRARIES} + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +target_include_directories(armor_detection_node + PRIVATE + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRECTORIES} +) +add_dependencies(armor_detection_node + roborts_msgs_generate_messages) + +#armor_detection_client +add_executable(${PROJECT_NAME}_client + armor_detection_client.cpp +) + +target_include_directories(armor_detection_client + PRIVATE + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRECTORIES} + ) + +target_link_libraries(armor_detection_client + PRIVATE + detection::tool + ${PROTOBUF_LIBRARIES} + ${catkin_LIBRARIES} +) +add_dependencies(armor_detection_client + roborts_msgs_generate_messages) diff --git a/roborts_detection/armor_detection/armor_detection_algorithms.h b/roborts_detection/armor_detection/armor_detection_algorithms.h new file mode 100755 index 00000000..ccd1a910 --- /dev/null +++ b/roborts_detection/armor_detection/armor_detection_algorithms.h @@ -0,0 +1,26 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DETECTION_ARMOR_DETECTION_ALGORITHMS_H +#define ROBORTS_DETECTION_ARMOR_DETECTION_ALGORITHMS_H + +/** + * Add your own algorithm harders here. + */ +#include "constraint_set/constraint_set.h" + +#endif // ROBORTS_DETECTION_ARMOR_DETECTION_ALGORITHMS_H \ No newline at end of file diff --git a/roborts_detection/armor_detection/armor_detection_base.h b/roborts_detection/armor_detection/armor_detection_base.h new file mode 100755 index 00000000..dc749b1b --- /dev/null +++ b/roborts_detection/armor_detection/armor_detection_base.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DETECTION_ARMOR_DETECTION_BASE_H +#define ROBORTS_DETECTION_ARMOR_DETECTION_BASE_H + +#include +#include "state/error_code.h" +#include "../util/cv_toolbox.h" + +namespace roborts_detection { + +using roborts_common::ErrorInfo; + +class ArmorDetectionBase { + public: + ArmorDetectionBase(std::shared_ptr cv_toolbox) + : cv_toolbox_(cv_toolbox) + { }; + virtual void LoadParam() = 0; + virtual ErrorInfo DetectArmor(bool &detected, cv::Point3f &target_3d) = 0; + virtual void SetThreadState(bool thread_state) = 0; + virtual ~ArmorDetectionBase() = default; + protected: + std::shared_ptr cv_toolbox_; +}; +} //namespace roborts_detection + +#endif //ROBORTS_DETECTION_ARMOR_DETECTION_BASE_H \ No newline at end of file diff --git a/roborts_detection/armor_detection/armor_detection_client.cpp b/roborts_detection/armor_detection/armor_detection_client.cpp new file mode 100755 index 00000000..dcddf042 --- /dev/null +++ b/roborts_detection/armor_detection/armor_detection_client.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include +#include +#include "roborts_msgs/ArmorDetectionAction.h" +#include + +int main(int argc, char **argv) { + ros::init(argc, argv, "armor_detection_node_test_client"); + + // create the action client + actionlib::SimpleActionClient ac("armor_detection_node_action", true); + //roborts_msgs::ArmorDetectionResult node_result; + ROS_INFO("Waiting for action server to start."); + ac.waitForServer(); + ROS_INFO("Start."); + roborts_msgs::ArmorDetectionGoal goal; + + char command = '0'; + + while (command != '4') { + std::cout << "**************************************************************************************" << std::endl; + std::cout << "*********************************please send a command********************************" << std::endl; + std::cout << "1: start the action" << std::endl + << "2: pause the action" << std::endl + << "3: stop the action" << std::endl + << "4: exit the program" << std::endl; + std::cout << "**************************************************************************************" << std::endl; + std::cout << "> "; + std::cin >> command; + if (command != '1' && command != '2' && command != '3' && command != '4') { + std::cout << "please inpugain!" << std::endl; + std::cout << "> "; + std::cin >> command; + } + + switch (command) { + //start thread. + case '1': + goal.command = 1; + ROS_INFO("I am running the request"); + ac.sendGoal(goal); + break; + //pause thread. + case '2': + goal.command = 2; + ROS_INFO("Action server will pause."); + ac.sendGoal(goal); + //stop thread. + case '3': + goal.command = 3; + ROS_INFO("I am cancelling the request"); + ac.cancelGoal(); + break; + default: + break; + } + } + return 0; +} diff --git a/roborts_detection/armor_detection/armor_detection_node.cpp b/roborts_detection/armor_detection/armor_detection_node.cpp new file mode 100755 index 00000000..c6d11695 --- /dev/null +++ b/roborts_detection/armor_detection/armor_detection_node.cpp @@ -0,0 +1,244 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include +#include "armor_detection_node.h" + +namespace roborts_detection { + +ArmorDetectionNode::ArmorDetectionNode(): + node_state_(roborts_common::IDLE), + demensions_(3), + initialized_(false), + detected_enemy_(false), + undetected_count_(0), + as_(nh_, "armor_detection_node_action", boost::bind(&ArmorDetectionNode::ActionCB, this, _1), false) { + initialized_ = false; + enemy_nh_ = ros::NodeHandle(); + if (Init().IsOK()) { + initialized_ = true; + node_state_ = roborts_common::IDLE; + } else { + ROS_ERROR("armor_detection_node initalized failed!"); + node_state_ = roborts_common::FAILURE; + } + as_.start(); +} + +ErrorInfo ArmorDetectionNode::Init() { + enemy_info_pub_ = enemy_nh_.advertise("cmd_gimbal_angle", 100); + ArmorDetectionAlgorithms armor_detection_param; + + std::string file_name = ros::package::getPath("roborts_detection") + "/armor_detection/config/armor_detection.prototxt"; + bool read_state = roborts_common::ReadProtoFromTextFile(file_name, &armor_detection_param); + if (!read_state) { + ROS_ERROR("Cannot open %s", file_name.c_str()); + return ErrorInfo(ErrorCode::DETECTION_INIT_ERROR); + } + gimbal_control_.Init(armor_detection_param.camera_gimbal_transform().offset_x(), + armor_detection_param.camera_gimbal_transform().offset_y(), + armor_detection_param.camera_gimbal_transform().offset_z(), + armor_detection_param.camera_gimbal_transform().offset_pitch(), + armor_detection_param.camera_gimbal_transform().offset_yaw(), + armor_detection_param.projectile_model_info().init_v(), + armor_detection_param.projectile_model_info().init_k()); + + //create the selected algorithms + std::string selected_algorithm = armor_detection_param.selected_algorithm(); + // create image receiver + cv_toolbox_ =std::make_shared(armor_detection_param.camera_name()); + // create armor detection algorithm + armor_detector_ = roborts_common::AlgorithmFactory>::CreateAlgorithm + (selected_algorithm, cv_toolbox_); + + undetected_armor_delay_ = armor_detection_param.undetected_armor_delay(); + if (armor_detector_ == nullptr) { + ROS_ERROR("Create armor_detector_ pointer failed!"); + return ErrorInfo(ErrorCode::DETECTION_INIT_ERROR); + } else + return ErrorInfo(ErrorCode::OK); +} + +void ArmorDetectionNode::ActionCB(const roborts_msgs::ArmorDetectionGoal::ConstPtr &data) { + roborts_msgs::ArmorDetectionFeedback feedback; + roborts_msgs::ArmorDetectionResult result; + bool undetected_msg_published = false; + + if(!initialized_){ + feedback.error_code = error_info_.error_code(); + feedback.error_msg = error_info_.error_msg(); + as_.publishFeedback(feedback); + as_.setAborted(result, feedback.error_msg); + ROS_INFO("Initialization Failed, Failed to execute action!"); + return; + } + + switch (data->command) { + case 1: + StartThread(); + break; + case 2: + PauseThread(); + break; + case 3: + StopThread(); + break; + default: + break; + } + ros::Rate rate(25); + while(ros::ok()) { + + if(as_.isPreemptRequested()) { + as_.setPreempted(); + return; + } + + { + std::lock_guard guard(mutex_); + if (undetected_count_ != 0) { + feedback.detected = true; + feedback.error_code = error_info_.error_code(); + feedback.error_msg = error_info_.error_msg(); + + feedback.enemy_pos.header.frame_id = "camera0"; + feedback.enemy_pos.header.stamp = ros::Time::now(); + + feedback.enemy_pos.pose.position.x = x_; + feedback.enemy_pos.pose.position.y = y_; + feedback.enemy_pos.pose.position.z = z_; + feedback.enemy_pos.pose.orientation.w = 1; + as_.publishFeedback(feedback); + undetected_msg_published = false; + } else if(!undetected_msg_published) { + feedback.detected = false; + feedback.error_code = error_info_.error_code(); + feedback.error_msg = error_info_.error_msg(); + + feedback.enemy_pos.header.frame_id = "camera0"; + feedback.enemy_pos.header.stamp = ros::Time::now(); + + feedback.enemy_pos.pose.position.x = 0; + feedback.enemy_pos.pose.position.y = 0; + feedback.enemy_pos.pose.position.z = 0; + feedback.enemy_pos.pose.orientation.w = 1; + as_.publishFeedback(feedback); + undetected_msg_published = true; + } + } + rate.sleep(); + } +} + +void ArmorDetectionNode::ExecuteLoop() { + undetected_count_ = undetected_armor_delay_; + + while(running_) { + usleep(1); + if (node_state_ == NodeState::RUNNING) { + cv::Point3f target_3d; + ErrorInfo error_info = armor_detector_->DetectArmor(detected_enemy_, target_3d); + { + std::lock_guard guard(mutex_); + x_ = target_3d.x; + y_ = target_3d.y; + z_ = target_3d.z; + error_info_ = error_info; + } + + if(detected_enemy_) { + float pitch, yaw; + gimbal_control_.Transform(target_3d, pitch, yaw); + + gimbal_angle_.yaw_mode = true; + gimbal_angle_.pitch_mode = false; + gimbal_angle_.yaw_angle = yaw * 0.7; + gimbal_angle_.pitch_angle = pitch; + + std::lock_guard guard(mutex_); + undetected_count_ = undetected_armor_delay_; + PublishMsgs(); + } else if(undetected_count_ != 0) { + + gimbal_angle_.yaw_mode = true; + gimbal_angle_.pitch_mode = false; + gimbal_angle_.yaw_angle = 0; + gimbal_angle_.pitch_angle = 0; + + undetected_count_--; + PublishMsgs(); + } + } else if (node_state_ == NodeState::PAUSE) { + std::unique_lock lock(mutex_); + condition_var_.wait(lock); + } + } +} + +void ArmorDetectionNode::PublishMsgs() { + enemy_info_pub_.publish(gimbal_angle_); +} + +void ArmorDetectionNode::StartThread() { + ROS_INFO("Armor detection node started!"); + running_ = true; + armor_detector_->SetThreadState(true); + if(node_state_ == NodeState::IDLE) { + armor_detection_thread_ = std::thread(&ArmorDetectionNode::ExecuteLoop, this); + } + node_state_ = NodeState::RUNNING; + condition_var_.notify_one(); +} + +void ArmorDetectionNode::PauseThread() { + ROS_INFO("Armor detection thread paused!"); + node_state_ = NodeState::PAUSE; +} + +void ArmorDetectionNode::StopThread() { + node_state_ = NodeState::IDLE; + running_ = false; + armor_detector_->SetThreadState(false); + if (armor_detection_thread_.joinable()) { + armor_detection_thread_.join(); + } +} + +ArmorDetectionNode::~ArmorDetectionNode() { + StopThread(); +} +} //namespace roborts_detection + +void SignalHandler(int signal){ + if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ + ros::shutdown(); + } +} + +int main(int argc, char **argv) { + signal(SIGINT, SignalHandler); + signal(SIGTERM,SignalHandler); + ros::init(argc, argv, "armor_detection_node", ros::init_options::NoSigintHandler); + roborts_detection::ArmorDetectionNode armor_detection; + ros::AsyncSpinner async_spinner(1); + async_spinner.start(); + ros::waitForShutdown(); + armor_detection.StopThread(); + return 0; +} + + diff --git a/roborts_detection/armor_detection/armor_detection_node.h b/roborts_detection/armor_detection/armor_detection_node.h new file mode 100755 index 00000000..c533f7c4 --- /dev/null +++ b/roborts_detection/armor_detection/armor_detection_node.h @@ -0,0 +1,119 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DETECTION_ARMOR_DETECTION_NODE_H +#define ROBORTS_DETECTION_ARMOR_DETECTION_NODE_H + +#include +#include +#include +#include + +#include +#include "actionlib/server/simple_action_server.h" +#include "roborts_msgs/GimbalAngle.h" +#include "roborts_msgs/GimbalRate.h" +#include "roborts_msgs/ArmorDetectionAction.h" + +#include "alg_factory/algorithm_factory.h" +#include "io/io.h" +#include "state/node_state.h" + +#include "cv_toolbox.h" + +#include "armor_detection_base.h" +#include "proto/armor_detection.pb.h" +#include "armor_detection_algorithms.h" +#include "gimbal_control.h" + +namespace roborts_detection { + +using roborts_common::NodeState; +using roborts_common::ErrorInfo; + +class ArmorDetectionNode { + public: + explicit ArmorDetectionNode(); + /** + * @brief Initializing armor detection algorithm. + * @return Return the error information. + */ + ErrorInfo Init(); + /** + * @brief Actionlib server call back function. + * @param data Command for control the algorithm thread. + */ + void ActionCB(const roborts_msgs::ArmorDetectionGoal::ConstPtr &data); + /** + * @brief Starting the armor detection thread. + */ + void StartThread(); + /** + * @brief Pausing the armor detection thread when received command 2 in action_lib callback function. + */ + void PauseThread(); + /** + * @brief Stopping armor detection thread. + */ + void StopThread(); + /** + * @brief Executing the armor detection algorithm. + */ + void ExecuteLoop(); + /** + * @brief Publishing enemy pose information that been calculated by the armor detection algorithm. + */ + void PublishMsgs(); + ~ArmorDetectionNode(); + protected: + private: + std::shared_ptr armor_detector_; + std::thread armor_detection_thread_; + unsigned int max_rotating_fps_; + unsigned int min_rotating_detected_count_; + unsigned int undetected_armor_delay_; + + //! state and error + NodeState node_state_; + ErrorInfo error_info_; + bool initialized_; + bool running_; + std::mutex mutex_; + std::condition_variable condition_var_; + unsigned int undetected_count_; + + //! enemy information + double x_; + double y_; + double z_; + bool detected_enemy_; + unsigned long demensions_; + + //ROS + ros::NodeHandle nh_; + ros::NodeHandle enemy_nh_; + ros::Publisher enemy_info_pub_; + std::shared_ptr cv_toolbox_; + actionlib::SimpleActionServer as_; + roborts_msgs::GimbalAngle gimbal_angle_; + + //! control model + GimbalContrl gimbal_control_; +}; +} //namespace roborts_detection + +#endif //ROBORTS_DETECTION_ARMOR_DETECTION_NODE_H diff --git a/roborts_detection/armor_detection/config/armor_detection.prototxt b/roborts_detection/armor_detection/config/armor_detection.prototxt new file mode 100755 index 00000000..ce4fe124 --- /dev/null +++ b/roborts_detection/armor_detection/config/armor_detection.prototxt @@ -0,0 +1,16 @@ +name: "constraint_set" +selected_algorithm: "constraint_set" +undetected_armor_delay: 10 +camera_name: "back_camera" +camera_gimbal_transform { + offset_x :0 + offset_y :10 + offset_z :9 + offset_pitch :-2.5 + offset_yaw : 4.5 +} + +projectile_model_info { + init_v: 15 # Launching projectile velocity + init_k: 0.026 +} \ No newline at end of file diff --git a/roborts_detection/armor_detection/constraint_set/CMakeLists.txt b/roborts_detection/armor_detection/constraint_set/CMakeLists.txt new file mode 100755 index 00000000..1fdc566c --- /dev/null +++ b/roborts_detection/armor_detection/constraint_set/CMakeLists.txt @@ -0,0 +1,29 @@ +project(constraint_set) + +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto + ConstraintSetProtoSrc + ConstraintSetProtoHds + ${ProtoFiles} + ) + +add_library(constraint_set + SHARED + ${ConstraintSetProtoSrc} + ${ConstraintSetProtoHds} + constraint_set.cpp + ) + +add_library(detection::constraint_set ALIAS constraint_set) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + detection::tool + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + ${catkin_INCLUDE_DIRS} + ) diff --git a/roborts_detection/armor_detection/constraint_set/config/constraint_set.prototxt b/roborts_detection/armor_detection/constraint_set/config/constraint_set.prototxt new file mode 100755 index 00000000..0574c3c7 --- /dev/null +++ b/roborts_detection/armor_detection/constraint_set/config/constraint_set.prototxt @@ -0,0 +1,33 @@ +threshold { + light_max_aspect_ratio: 20.0 + light_min_area: 1 + light_max_angle: 20.0 + light_max_angle_diff: 10.0 + + armor_max_angle: 15.0 + armor_min_area: 100 + armor_max_aspect_ratio: 4 + armor_max_pixel_val: 150.0 + armor_max_stddev: 90 + armor_max_mean: 90 + + color_thread: 100 + blue_thread: 90 + red_thread: 50 +} + +signal_recognization { + max_wait_fps: 100 + min_pulse_angle: 0.4 + min_num: 5 +} + +armor_size { + width: 120 # mm + height: 60 # mm +} + +using_hsv: false +enable_debug: true +enable_neon: false +enemy_color: BLUE diff --git a/roborts_detection/armor_detection/constraint_set/constraint_set.cpp b/roborts_detection/armor_detection/constraint_set/constraint_set.cpp new file mode 100755 index 00000000..927dc4e4 --- /dev/null +++ b/roborts_detection/armor_detection/constraint_set/constraint_set.cpp @@ -0,0 +1,474 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#include +#include + +#include "constraint_set.h" + +#include "timer/timer.h" +#include "io/io.h" + +namespace roborts_detection { + +ConstraintSet::ConstraintSet(std::shared_ptr cv_toolbox): + ArmorDetectionBase(cv_toolbox){ + filter_x_count_ = 0; + filter_y_count_ = 0; + filter_z_count_ = 0; + filter_distance_count_ = 0; + filter_pitch_count_ = 0; + filter_yaw_count_ = 0; + read_index_ = -1; + detection_time_ = 0; + thread_running_ = false; + + LoadParam(); + error_info_ = ErrorInfo(roborts_common::OK); +} + +void ConstraintSet::LoadParam() { + //read parameters + ConstraintSetConfig constraint_set_config_; + std::string file_name = ros::package::getPath("roborts_detection") + \ + "/armor_detection/constraint_set/config/constraint_set.prototxt"; + bool read_state = roborts_common::ReadProtoFromTextFile(file_name, &constraint_set_config_); + ROS_ASSERT_MSG(read_state, "Cannot open %s", file_name.c_str()); + + enable_debug_ = constraint_set_config_.enable_debug(); + enemy_color_ = constraint_set_config_.enemy_color(); + using_hsv_ = constraint_set_config_.using_hsv(); + + //armor info + float armor_width = constraint_set_config_.armor_size().width(); + float armor_height = constraint_set_config_.armor_size().height(); + SolveArmorCoordinate(armor_width, armor_height); + + //algorithm threshold parameters + light_max_aspect_ratio_ = constraint_set_config_.threshold().light_max_aspect_ratio(); + light_min_area_ = constraint_set_config_.threshold().light_min_area(); + light_max_angle_ = constraint_set_config_.threshold().light_max_angle(); + light_max_angle_diff_ = constraint_set_config_.threshold().light_max_angle_diff(); + armor_max_angle_ = constraint_set_config_.threshold().armor_max_angle(); + armor_min_area_ = constraint_set_config_.threshold().armor_min_area(); + armor_max_aspect_ratio_ = constraint_set_config_.threshold().armor_max_aspect_ratio(); + armor_max_pixel_val_ = constraint_set_config_.threshold().armor_max_pixel_val(); + armor_max_stddev_ = constraint_set_config_.threshold().armor_max_stddev(); + armor_max_mean_ = constraint_set_config_.threshold().armor_max_mean(); + + color_thread_ = constraint_set_config_.threshold().color_thread(); + blue_thread_ = constraint_set_config_.threshold().blue_thread(); + red_thread_ = constraint_set_config_.threshold().red_thread(); + + int get_intrinsic_state = -1; + int get_distortion_state = -1; + + while ((get_intrinsic_state < 0) || (get_distortion_state < 0)) { + ROS_WARN("Wait for camera driver launch %d", get_intrinsic_state); + usleep(50000); + ros::spinOnce(); + get_intrinsic_state = cv_toolbox_->GetCameraMatrix(intrinsic_matrix_); + get_distortion_state = cv_toolbox_->GetCameraDistortion(distortion_coeffs_); + } +} + + + +ErrorInfo ConstraintSet::DetectArmor(bool &detected, cv::Point3f &target_3d) { + std::vector lights; + std::vector armors; + + auto img_begin = std::chrono::high_resolution_clock::now(); + bool sleep_by_diff_flag = true; + while (true) { + // Ensure exit this thread while call Ctrl-C + if (!thread_running_) { + ErrorInfo error_info(ErrorCode::STOP_DETECTION); + return error_info; + } + read_index_ = cv_toolbox_->NextImage(src_img_); + if (read_index_ < 0) { + // Reducing lock and unlock when accessing function 'NextImage' + if (detection_time_ == 0) { + usleep(20000); + continue; + } else { + double capture_time = 0; + cv_toolbox_->GetCaptureTime(capture_time); + if (capture_time == 0) { + // Make sure the driver is launched and the image callback is called + usleep(20000); + continue; + } else if (capture_time > detection_time_ && sleep_by_diff_flag) { +// ROS_WARN("time sleep %lf", (capture_time - detection_time_)); + usleep((unsigned int)(capture_time - detection_time_)); + sleep_by_diff_flag = false; + continue; + } else { + //For real time request when image call back called, the function 'NextImage' should be called. + usleep(500); + continue; + } + } + } else { + break; + } + } + /*ROS_WARN("time get image: %lf", std::chrono::duration> + (std::chrono::high_resolution_clock::now() - img_begin).count());*/ + + auto detection_begin = std::chrono::high_resolution_clock::now(); + + cv::cvtColor(src_img_, gray_img_, CV_BGR2GRAY); + if (enable_debug_) { + show_lights_before_filter_ = src_img_.clone(); + show_lights_after_filter_ = src_img_.clone(); + show_armors_befor_filter_ = src_img_.clone(); + show_armors_after_filter_ = src_img_.clone(); + cv::waitKey(1); + } + + DetectLights(src_img_, lights); + FilterLights(lights); + PossibleArmors(lights, armors); + FilterArmors(armors); + if(!armors.empty()) { + detected = true; + ArmorInfo final_armor = SlectFinalArmor(armors); + cv_toolbox_->DrawRotatedRect(src_img_, armors[0].rect, cv::Scalar(0, 255, 0), 2); + CalcControlInfo(final_armor, target_3d); + } else + detected = false; + if(enable_debug_) { + cv::imshow("relust_img_", src_img_); + } + + lights.clear(); + armors.clear(); + cv_toolbox_->ReadComplete(read_index_); + ROS_INFO("read complete"); + detection_time_ = std::chrono::duration> + (std::chrono::high_resolution_clock::now() - detection_begin).count(); + + return error_info_; +} + +void ConstraintSet::DetectLights(const cv::Mat &src, std::vector &lights) { + //std::cout << "********************************************DetectLights********************************************" << std::endl; + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + cv::dilate(src, src, element, cv::Point(-1, -1), 1); + cv::Mat binary_brightness_img, binary_light_img, binary_color_img; + if(using_hsv_) { + binary_color_img = cv_toolbox_->DistillationColor(src, enemy_color_, using_hsv_); + cv::threshold(gray_img_, binary_brightness_img, color_thread_, 255, CV_THRESH_BINARY); + }else { + auto light = cv_toolbox_->DistillationColor(src, enemy_color_, using_hsv_); + cv::threshold(gray_img_, binary_brightness_img, color_thread_, 255, CV_THRESH_BINARY); + float thresh; + if (enemy_color_ == BLUE) + thresh = blue_thread_; + else + thresh = red_thread_; + cv::threshold(light, binary_color_img, thresh, 255, CV_THRESH_BINARY); + if(enable_debug_) + cv::imshow("light", light); + } + //binary_light_img = binary_color_img & binary_brightness_img; + if (enable_debug_) { + cv::imshow("binary_brightness_img", binary_brightness_img); + //cv::imshow("binary_light_img", binary_light_img); + cv::imshow("binary_color_img", binary_color_img); + } + + auto contours_light = cv_toolbox_->FindContours(binary_color_img); + auto contours_brightness = cv_toolbox_->FindContours(binary_brightness_img); + + lights.reserve(contours_light.size()); + lights_info_.reserve(contours_light.size()); + // TODO: To be optimized + //std::vector is_processes(contours_light.size()); + for (unsigned int i = 0; i < contours_brightness.size(); ++i) { + for (unsigned int j = 0; j < contours_light.size(); ++j) { + + if (cv::pointPolygonTest(contours_light[j], contours_brightness[i][0], false) >= 0.0) { + cv::RotatedRect single_light = cv::minAreaRect(contours_brightness[i]); + cv::Point2f vertices_point[4]; + single_light.points(vertices_point); + LightInfo light_info(vertices_point); + + if (enable_debug_) + cv_toolbox_->DrawRotatedRect(show_lights_before_filter_, single_light, cv::Scalar(0, 255, 0), 2, light_info.angle_); + single_light.angle = light_info.angle_; + lights.push_back(single_light); + break; + } + } + } + + if (enable_debug_) + cv::imshow("show_lights_before_filter", show_lights_before_filter_); + + auto c = cv::waitKey(1); + if (c == 'a') { + cv::waitKey(0); + } +} + + +void ConstraintSet::FilterLights(std::vector &lights) { + //std::cout << "********************************************FilterLights********************************************" << std::endl; + std::vector rects; + rects.reserve(lights.size()); + + for (const auto &light : lights) { + float angle; + auto light_aspect_ratio = + std::max(light.size.width, light.size.height) / std::min(light.size.width, light.size.height); + //https://stackoverflow.com/questions/15956124/minarearect-angles-unsure-about-the-angle-returned/21427814#21427814 + if(light.size.width < light.size.height) { + angle = light.angle; // -light.angle + } else + angle = light.angle; // light.angle + 90 + //std::cout << "light angle: " << angle << std::endl; + //std::cout << "light_aspect_ratio: " << light_aspect_ratio << std::endl; + //std::cout << "light_area: " << light.size.area() << std::endl; + if (light_aspect_ratio < light_max_aspect_ratio_ && + light.size.area() >= light_min_area_) { //angle < light_max_angle_ && + rects.push_back(light); + if (enable_debug_) + cv_toolbox_->DrawRotatedRect(show_lights_after_filter_, light, cv::Scalar(0, 255, 0), 2, angle); + } + } + if (enable_debug_) + cv::imshow("lights_after_filter", show_lights_after_filter_); + + lights = rects; +} + +void ConstraintSet::PossibleArmors(const std::vector &lights, std::vector &armors) { + //std::cout << "********************************************PossibleArmors********************************************" << std::endl; + for (unsigned int i = 0; i < lights.size(); i++) { + for (unsigned int j = i + 1; j < lights.size(); j++) { + cv::RotatedRect light1 = lights[i]; + cv::RotatedRect light2 = lights[j]; + auto edge1 = std::minmax(light1.size.width, light1.size.height); + auto edge2 = std::minmax(light2.size.width, light2.size.height); + auto lights_dis = std::sqrt((light1.center.x - light2.center.x) * (light1.center.x - light2.center.x) + + (light1.center.y - light2.center.y) * (light1.center.y - light2.center.y)); + auto center_angle = std::atan(std::abs(light1.center.y - light2.center.y) / std::abs(light1.center.x - light2.center.x)) * 180 / CV_PI; + center_angle = center_angle > 90 ? 180 - center_angle : center_angle; + //std::cout << "center_angle: " << center_angle << std::endl; + + cv::RotatedRect rect; + rect.angle = static_cast(center_angle); + rect.center.x = (light1.center.x + light2.center.x) / 2; + rect.center.y = (light1.center.y + light2.center.y) / 2; + float armor_width = std::abs(static_cast(lights_dis) - std::max(edge1.first, edge2.first)); + float armor_height = std::max(edge1.second, edge2.second); + + rect.size.width = std::max(armor_width, armor_height); + rect.size.height = std::min(armor_width, armor_height); + + float light1_angle = light1.angle; //light1.size.width < light1.size.height ? -light1.angle : light1.angle + 90 + float light2_angle = light2.angle; //light2.size.width < light2.size.height ? -light2.angle : light2.angle + 90 + //std::cout << "light1_angle: " << light1_angle << std::endl; + //std::cout << "light2_angle: " << light2_angle << std::endl; + + if (enable_debug_) { + std::cout << "*******************************" << std::endl; + std::cout << "light_angle_diff_: " << std::abs(light1_angle - light2_angle) << std::endl; + std::cout << "radio: " << std::max(edge1.second, edge2.second)/std::min(edge1.second, edge2.second) << std::endl; + std::cout << "armor_angle_: " << std::abs(center_angle) << std::endl; + std::cout << "armor_aspect_ratio_: " << rect.size.width / (float) (rect.size.height) << std::endl; + std::cout << "armor_area_: " << std::abs(rect.size.area()) << std::endl; + std::cout << "armor_pixel_val_: " << (float)(gray_img_.at(static_cast(rect.center.y), static_cast(rect.center.x))) << std::endl; + std::cout << "pixel_y" << static_cast(rect.center.y) << std::endl; + std::cout << "pixel_x" << static_cast(rect.center.x) << std::endl; + } + // + auto angle_diff = std::abs(light1_angle - light2_angle); + // Avoid incorrect calculation at 180 and 0. + if (angle_diff > 175) { + angle_diff = 180 -angle_diff; + } + + if (angle_diff < light_max_angle_diff_ && + std::max(edge1.second, edge2.second)/std::min(edge1.second, edge2.second) < 2.0 && + rect.size.width / (rect.size.height) < armor_max_aspect_ratio_ && + std::abs(rect.size.area()) > armor_min_area_ && + gray_img_.at(static_cast(rect.center.y), static_cast(rect.center.x)) + < armor_max_pixel_val_) { //std::abs(center_angle) < armor_max_angle_ && + + if (light1.center.x < light2.center.x) { + std::vector armor_points; + CalcArmorInfo(armor_points, light1, light2); + armors.emplace_back(ArmorInfo(rect, armor_points)); + if (enable_debug_) + cv_toolbox_->DrawRotatedRect(show_armors_befor_filter_, rect, cv::Scalar(0, 255, 0), 2); + armor_points.clear(); + } else { + std::vector armor_points; + CalcArmorInfo(armor_points, light2, light1); + armors.emplace_back(ArmorInfo(rect, armor_points)); + if (enable_debug_) + cv_toolbox_->DrawRotatedRect(show_armors_befor_filter_, rect, cv::Scalar(0, 255, 0), 2); + armor_points.clear(); + } + } + } + } + if (enable_debug_) + cv::imshow("armors_before_filter", show_armors_befor_filter_); +} + +void ConstraintSet::FilterArmors(std::vector &armors) { + //std::cout << "********************************************FilterArmors********************************************" << std::endl; + cv::Mat mask = cv::Mat::zeros(gray_img_.size(), CV_8UC1); + for (auto armor_iter = armors.begin(); armor_iter != armors.end();) { + cv::Point pts[4]; + for (unsigned int i = 0; i < 4; i++) { + pts[i].x = (int) armor_iter->vertex[i].x; + pts[i].y = (int) armor_iter->vertex[i].y; + } + cv::fillConvexPoly(mask, pts, 4, cv::Scalar(255), 8, 0); + + cv::Mat mat_mean; + cv::Mat mat_stddev; + cv::meanStdDev(gray_img_, mat_mean, mat_stddev, mask); + + auto stddev = mat_stddev.at(0, 0); + auto mean = mat_mean.at(0, 0); + //std::cout << "stddev: " << stddev << std::endl; + //std::cout << "mean: " << mean << std::endl; + + if (stddev > armor_max_stddev_ || mean > armor_max_mean_) { + armor_iter = armors.erase(armor_iter); + } else { + armor_iter++; + } + } + + // nms + std::vector is_armor(armors.size(), true); + for (int i = 0; i < armors.size() && is_armor[i] == true; i++) { + for (int j = i + 1; j < armors.size() && is_armor[j]; j++) { + float dx = armors[i].rect.center.x - armors[j].rect.center.x; + float dy = armors[i].rect.center.y - armors[j].rect.center.y; + float dis = std::sqrt(dx * dx + dy * dy); + if (dis < armors[i].rect.size.width + armors[j].rect.size.width) { + if (armors[i].rect.angle > armors[j].rect.angle) { + is_armor[i] = false; + //std::cout << "i: " << i << std::endl; + } else { + is_armor[j] = false; + //std::cout << "j: " << j << std::endl; + } + } + } + } + //std::cout << armors.size() << std::endl; + for (unsigned int i = 0; i < armors.size(); i++) { + if (!is_armor[i]) { + armors.erase(armors.begin() + i); + is_armor.erase(is_armor.begin() + i); + //std::cout << "index: " << i << std::endl; + } else if (enable_debug_) { + cv_toolbox_->DrawRotatedRect(show_armors_after_filter_, armors[i].rect, cv::Scalar(0, 255, 0), 2); + } + } + if (enable_debug_) + cv::imshow("armors_after_filter", show_armors_after_filter_); +} + +ArmorInfo ConstraintSet::SlectFinalArmor(std::vector &armors) { + std::sort(armors.begin(), + armors.end(), + [](const ArmorInfo &p1, const ArmorInfo &p2) { return p1.rect.size.area() > p2.rect.size.area(); }); + + return armors[0]; +} + +void ConstraintSet::CalcControlInfo(const ArmorInfo & armor, cv::Point3f &target_3d) { + cv::Mat rvec; + cv::Mat tvec; + cv::solvePnP(armor_points_, + armor.vertex, + intrinsic_matrix_, + distortion_coeffs_, + rvec, + tvec); + target_3d = cv::Point3f(tvec); + +} + +void ConstraintSet::CalcArmorInfo(std::vector &armor_points, + cv::RotatedRect left_light, + cv::RotatedRect right_light) { + cv::Point2f left_points[4], right_points[4]; + left_light.points(left_points); + right_light.points(right_points); + + cv::Point2f right_lu, right_ld, lift_ru, lift_rd; + std::sort(left_points, left_points + 4, [](const cv::Point2f &p1, const cv::Point2f &p2) { return p1.x < p2.x; }); + std::sort(right_points, right_points + 4, [](const cv::Point2f &p1, const cv::Point2f &p2) { return p1.x < p2.x; }); + if (right_points[0].y < right_points[1].y) { + right_lu = right_points[0]; + right_ld = right_points[1]; + } else { + right_lu = right_points[1]; + right_ld = right_points[0]; + } + + if (left_points[2].y < left_points[3].y) { + lift_ru = left_points[2]; + lift_rd = left_points[3]; + } else { + lift_ru = left_points[3]; + lift_rd = left_points[2]; + } + armor_points.push_back(lift_ru); + armor_points.push_back(right_lu); + armor_points.push_back(right_ld); + armor_points.push_back(lift_rd); + +} + +void ConstraintSet::SolveArmorCoordinate(const float width, + const float height) { + armor_points_.emplace_back(cv::Point3f(-width/2, height/2, 0.0)); + armor_points_.emplace_back(cv::Point3f(width/2, height/2, 0.0)); + armor_points_.emplace_back(cv::Point3f(width/2, -height/2, 0.0)); + armor_points_.emplace_back(cv::Point3f(-width/2, -height/2, 0.0)); +} + +void ConstraintSet::SignalFilter(double &new_num, double &old_num, unsigned int &filter_count, double max_diff) { + if(fabs(new_num - old_num) > max_diff && filter_count < 2) { + filter_count++; + new_num += max_diff; + } else { + filter_count = 0; + old_num = new_num; + } +} + +void ConstraintSet::SetThreadState(bool thread_state) { + thread_running_ = thread_state; +} + +ConstraintSet::~ConstraintSet() { + +} +} //namespace roborts_detection diff --git a/roborts_detection/armor_detection/constraint_set/constraint_set.h b/roborts_detection/armor_detection/constraint_set/constraint_set.h new file mode 100755 index 00000000..15ce2807 --- /dev/null +++ b/roborts_detection/armor_detection/constraint_set/constraint_set.h @@ -0,0 +1,261 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DETECTION_ARMOR_DETECTION_CONSTRAINT_SET_H +#define ROBORTS_DETECTION_ARMOR_DETECTION_CONSTRAINT_SET_H + +//system include +#include +#include + +#include + +#include "alg_factory/algorithm_factory.h" +#include "state/error_code.h" + +#include "cv_toolbox.h" + +#include "../armor_detection_base.h" + +#include "proto/constraint_set.pb.h" +#include "constraint_set.h" +namespace roborts_detection { + +using roborts_common::ErrorCode; +using roborts_common::ErrorInfo; + +enum State { + INITIALIZED = 0, + RUNNING = 1, + FAILED = 2, + STOPED = 3 +}; + +struct LightInfo { + + explicit LightInfo(cv::Point2f vertices[]) { + auto edge_1 = std::pow(vertices[0].x - vertices[1].x, 2) + + std::pow(vertices[0].y - vertices[1].y, 2); + auto edge_2 = std::pow(vertices[1].x - vertices[2].x, 2) + + std::pow(vertices[1].y - vertices[2].y, 2); + + if (edge_1 > edge_2) { + width_ = (float)std::sqrt(edge_1); + height_ = (float)std::sqrt(edge_2); + + if (vertices[0].y < vertices[1].y) { + angle_ = std::atan2(vertices[1].y - vertices[0].y, vertices[1].x - vertices[0].x); + } else { + angle_ = std::atan2(vertices[0].y - vertices[1].y, vertices[0].x - vertices[1].x); + } + + } else { + width_ = (float)std::sqrt(edge_2); + height_ = (float)std::sqrt(edge_1); + + if (vertices[2].y < vertices[1].y) { + angle_ = std::atan2(vertices[1].y - vertices[2].y, vertices[1].x - vertices[2].x); + } else { + angle_ = std::atan2(vertices[2].y - vertices[1].y, vertices[2].x - vertices[1].x); + } + + } + + angle_ = (float)(angle_*180.0 / 3.1415926); + area_ = width_ * height_; + aspect_ratio_ = width_ / height_; + center_.x = (vertices[1].x + vertices[3].x) / 2; + center_.y = (vertices[1].y + vertices[3].y) / 2; + vertices_.push_back(vertices[0]); + vertices_.push_back(vertices[1]); + vertices_.push_back(vertices[2]); + vertices_.push_back(vertices[3]); + } + + public: + //! Light area + float area_; + //! Light angle, come from the long edge's slope + float angle_; + //! Light center + cv::Point2f center_; + //! Light aspect ratio = width_/height_ + float aspect_ratio_; + //! Light width + float width_; + //! Light height + float height_; + //! Light vertices + std::vector vertices_; +}; + +/** + * This class describes the armor information, including maximum bounding box, vertex, standard deviation. + */ +class ArmorInfo { + public: + ArmorInfo(cv::RotatedRect armor_rect, std::vector armor_vertex, float armor_stddev = 0.0) { + rect = armor_rect; + vertex = armor_vertex; + stddev = armor_stddev; + } + public: + cv::RotatedRect rect; + std::vector vertex; + float stddev; +}; + +/** + * @brief This class achieved functions that can help to detect armors of RoboMaster vehicle. + */ +class ConstraintSet : public ArmorDetectionBase { + public: + ConstraintSet(std::shared_ptr cv_toolbox); + /** + * @brief Loading parameters from .prototxt file. + */ + void LoadParam() override; + /** + * @brief The entrance function of armor detection. + * @param translation Translation information of the armor relative to the camera. + * @param rotation Rotation information of the armor relative to the camera. + */ + ErrorInfo DetectArmor(bool &detected, cv::Point3f &target_3d) override; + /** + * @brief Detecting lights on the armors. + * @param src Input image + * @param lights Output lights information + */ + void DetectLights(const cv::Mat &src, std::vector &lights); + /** + * @brief Filtering the detected lights. + * @param lights Filtered lights + */ + void FilterLights(std::vector &lights); + /** + * @brief Finding possible armors. + * @param lights Take lights information as input. + * @param armors Possible armors + */ + void PossibleArmors(const std::vector &lights, std::vector &armors); + /** + * @brief Filtering Detected armors by standard deviation and non-maximum suppression(nms). + * @param armors Result armors + */ + void FilterArmors(std::vector &armors); + /** + * @brief Slecting final armor as the target armor which we will be shot. + * @param Input armors + */ + ArmorInfo SlectFinalArmor(std::vector &armors); + /** + * + * @param armor + * @param distance + * @param pitch + * @param yaw + * @param bullet_speed + */ + void CalcControlInfo(const ArmorInfo & armor, cv::Point3f &target_3d); + + /** + * @brief Using two lights(left light and right light) to calculate four points of armor. + * @param armor_points Out put + * @param left_light Rotated rect of left light + * @param right_light Rotated rectangles of right light + */ + void CalcArmorInfo(std::vector &armor_points, cv::RotatedRect left_light, cv::RotatedRect right_light); + /** + * @brief Calculating the coordinates of the armor by its width and height. + * @param width Armor width + * @param height Armor height + */ + void SolveArmorCoordinate(const float width, const float height); + /** + * + */ + void SignalFilter(double &new_num, double &old_num,unsigned int &filter_count, double max_diff); + + void SetThreadState(bool thread_state) override; + /** + * @brief Destructor + */ + ~ConstraintSet() final; + private: + ErrorInfo error_info_; + unsigned int filter_x_count_; + unsigned int filter_y_count_; + unsigned int filter_z_count_; + unsigned int filter_distance_count_; + unsigned int filter_pitch_count_; + unsigned int filter_yaw_count_; + + cv::Mat src_img_; + cv::Mat gray_img_; + //! Camera intrinsic matrix + cv::Mat intrinsic_matrix_; + //! Camera distortion Coefficient + cv::Mat distortion_coeffs_; + //! Read image index + int read_index_; + //! detection time + double detection_time_; + + // Parameters come form .prototxt file + bool enable_debug_; + bool using_hsv_; + unsigned int enemy_color_; + + //! Use for debug + cv::Mat show_lights_before_filter_; + cv::Mat show_lights_after_filter_; + cv::Mat show_armors_befor_filter_; + cv::Mat show_armors_after_filter_; + + //! armor info + std::vector armor_points_; + + //! Filter lights + std::vector lights_info_; + float light_max_aspect_ratio_; + float light_min_area_; + float light_max_angle_; + float light_max_angle_diff_; + + //! Filter armor + float armor_max_angle_; + float armor_min_area_; + float armor_max_aspect_ratio_; + float armor_max_pixel_val_; + float armor_max_mean_; + float armor_max_stddev_; + + float color_thread_; + float blue_thread_; + float red_thread_; + + bool thread_running_; + + //ros + ros::NodeHandle nh; +}; + +roborts_common::REGISTER_ALGORITHM(ArmorDetectionBase, "constraint_set", ConstraintSet, std::shared_ptr); + +} //namespace roborts_detection + +#endif // AOTO_PILOT_DETECTION_ARMOR_DETECTION_CONSTRAINT_SET_H diff --git a/roborts_detection/armor_detection/constraint_set/proto/constraint_set.proto b/roborts_detection/armor_detection/constraint_set/proto/constraint_set.proto new file mode 100755 index 00000000..b86871e1 --- /dev/null +++ b/roborts_detection/armor_detection/constraint_set/proto/constraint_set.proto @@ -0,0 +1,47 @@ +syntax = "proto2"; +package roborts_detection; + +message Threshold { + //for light + required float light_max_aspect_ratio = 1; + required float light_min_area = 2; + required float light_max_angle = 3; + required float light_max_angle_diff = 4; + //for armor + required float armor_max_angle = 5; + required float armor_min_area = 6; + required float armor_max_aspect_ratio = 7; + required float armor_max_pixel_val = 8; + required float armor_max_stddev = 9; + required float armor_max_mean = 10; + + required float color_thread = 11; + required float blue_thread = 12; + required float red_thread = 13; +} + +message ArmorSize { + required float width = 1; + required float height = 2; +} + +enum EnemyColor { + BLUE = 0; + RED = 1; +} + +message SignalRecognization { + required uint32 max_wait_fps = 1; + required float min_pulse_angle = 2; + required uint32 min_num = 3; +} + +message ConstraintSetConfig { + required bool enable_debug = 1; + required bool enable_neon = 2; + required bool using_hsv = 3; + required Threshold threshold = 4; + required ArmorSize armor_size = 5; + required EnemyColor enemy_color = 6; + required SignalRecognization signal_recognization = 7; +} diff --git a/roborts_detection/armor_detection/gimbal_control.cpp b/roborts_detection/armor_detection/gimbal_control.cpp new file mode 100644 index 00000000..3ab86b27 --- /dev/null +++ b/roborts_detection/armor_detection/gimbal_control.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + + ***************************************************************************/ +#include +#include + +#include "gimbal_control.h" + +namespace roborts_detection { + +void GimbalContrl::Init(float x,float y,float z,float pitch,float yaw, float init_v, float init_k) { + offset_.x = x; + offset_.y = y; + offset_.z = z; + offset_pitch_ = pitch; + offset_yaw_ = yaw; + init_v_ = init_v; + init_k_ = init_k; +} + +//air friction is considered +float GimbalContrl::BulletModel(float x, float v, float angle) { //x:m,v:m/s,angle:rad + float t, y; + t = (float)((exp(init_k_ * x) - 1) / (init_k_ * v * cos(angle))); + y = (float)(v * sin(angle) * t - GRAVITY * t * t / 2); + return y; +} + +//x:distance , y: height +float GimbalContrl::GetPitch(float x, float y, float v) { + float y_temp, y_actual, dy; + float a; + y_temp = y; + // by iteration + for (int i = 0; i < 20; i++) { + a = (float) atan2(y_temp, x); + y_actual = BulletModel(x, v, a); + dy = y - y_actual; + y_temp = y_temp + dy; + if (fabsf(dy) < 0.001) { + break; + } + //printf("iteration num %d: angle %f,temp target y:%f,err of y:%f\n",i+1,a*180/3.1415926535,yTemp,dy); + } + return a; + +} + +void GimbalContrl::Transform(cv::Point3f &postion, float &pitch, float &yaw) { + pitch = + -GetPitch((postion.z + offset_.z) / 100, -(postion.y + offset_.y) / 100, 15) + (float)(offset_pitch_ * 3.1415926535 / 180); + //yaw positive direction :anticlockwise + yaw = -(float) (atan2(postion.x + offset_.x, postion.z + offset_.z)) + (float)(offset_yaw_ * 3.1415926535 / 180); +} + +} // roborts_detection + + + diff --git a/roborts_detection/armor_detection/gimbal_control.h b/roborts_detection/armor_detection/gimbal_control.h new file mode 100644 index 00000000..a790bf3a --- /dev/null +++ b/roborts_detection/armor_detection/gimbal_control.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_DETECTION_GIMBAL_CONTROL_H +#define ROBORTS_DETECTION_GIMBAL_CONTROL_H +#include +#include + +namespace roborts_detection{ + +const double PI = 3.1415926535; +const float GRAVITY = 9.78; + + +/** + * @brief The class can make a transformation: the 3D position of enemy --> pitch,yaw angle of gimbal. + * For more derails, see projectile_model.pdf + * TODO: add enemy motion estimation + */ + +class GimbalContrl +{ + private: + /** + * @brief Calculate the actual y value with air resistance + * @param x the distanc + * @param v Projectile velocity + * @param angle Pitch angle + * @return The actual y value in the gimbal coordinate + */ + float BulletModel(float x,float v,float angle); + /** + * @brief Get the gimbal control angle + * @param x Distance from enemy(the armor selected to shoot) to gimbal + * @param y Value of y in gimbal coordinate. + * @param v Projectile velocity + * @return Gimbal pitch angle + */ + float GetPitch(float x,float y,float v); + + public: + /** + * @brief Init the Transformation matrix from camera to gimbal //TODO: write in ros tf + * @param x Translate x + * @param y Translate y + * @param z Translate z + * @param pitch Rotate pitch + * @param yaw Rotate yaw + */ + void Init(float x,float y,float z,float pitch,float yaw, float init_v, float init_k); + /** + * @brief Get the gimbal control info. + * @param postion Enemy position(actually it should be the target armor). + * @param pitch Input and output actual pitch angle + * @param yaw Input and output actual yaw angle + */ + void Transform(cv::Point3f &postion,float &pitch,float &yaw); + + private: + //! Transformation matrix between camera coordinate system and gimbal coordinate system. + //! Translation unit: cm + cv::Point3f offset_; + //! Rotation matrix unit: degree + float offset_pitch_; + float offset_yaw_; + + //! Initial value + float init_v_; + float init_k_; + +}; + +} //namespace roborts_detection + +#endif //ROBORTS_DETECTION_GIMBAL_CONTROL_H diff --git a/roborts_detection/armor_detection/proto/armor_detection.proto b/roborts_detection/armor_detection/proto/armor_detection.proto new file mode 100755 index 00000000..20d4863f --- /dev/null +++ b/roborts_detection/armor_detection/proto/armor_detection.proto @@ -0,0 +1,24 @@ +syntax = "proto2"; +package roborts_detection; + +message CameraGimbalTransform{ + required float offset_x = 1; + required float offset_y = 2; + required float offset_z = 3; + required float offset_pitch = 4; + required float offset_yaw = 5; +} + +message ProjectileModelInfo { + optional float init_v = 1; + optional float init_k = 2; +} + +message ArmorDetectionAlgorithms { + repeated string name = 1; + optional string selected_algorithm = 2; + optional uint32 undetected_armor_delay = 3; + optional string camera_name = 4; + required CameraGimbalTransform camera_gimbal_transform= 5; + optional ProjectileModelInfo projectile_model_info = 6; +} diff --git a/roborts_detection/cmake_module/FindEigen3.cmake b/roborts_detection/cmake_module/FindEigen3.cmake new file mode 100755 index 00000000..239c1bbe --- /dev/null +++ b/roborts_detection/cmake_module/FindEigen3.cmake @@ -0,0 +1,86 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/roborts_detection/cmake_module/FindProtoBuf.cmake b/roborts_detection/cmake_module/FindProtoBuf.cmake new file mode 100755 index 00000000..3462e7be --- /dev/null +++ b/roborts_detection/cmake_module/FindProtoBuf.cmake @@ -0,0 +1,84 @@ +# Finds Google Protocol Buffers library and compilers and extends +# the standard cmake script + +find_package(Protobuf REQUIRED) +list(APPEND RRTS_INCLUDE_DIRS PUBLIC ${PROTOBUF_INCLUDE_DIR}) +list(APPEND RRTS_LINKER_LIBS PUBLIC ${PROTOBUF_LIBRARIES}) + +# As of Ubuntu 14.04 protoc is no longer a part of libprotobuf-dev package +# and should be installed separately as in: sudo apt-get install protobuf-compiler +if(EXISTS ${PROTOBUF_PROTOC_EXECUTABLE}) + message(STATUS "Found PROTOBUF Compiler: ${PROTOBUF_PROTOC_EXECUTABLE}") +else() + message(FATAL_ERROR "Could not find PROTOBUF Compiler") +endif() + +if (PROTOBUF_FOUND) + message ("protobuf ${PROTOBUF_LIBRARIES} found") +else () + message (FATAL_ERROR "Cannot find protobuf") +endif () + +# place where to generate protobuf sources +#set(proto_gen_folder "${PROJECT_BINARY_DIR}/include/caffe/proto") +#include_directories("${PROJECT_BINARY_DIR}/include") + +set(PROTOBUF_GENERATE_CPP_APPEND_PATH TRUE) + +################################################################################################ +# Modification of standard 'protobuf_generate_cpp()' with output dir parameter +# Usage: +# caffe_protobuf_generate_cpp_py( ) +function(rrts_protobuf_generate_cpp output_dir proto_srcs proto_hdrs) + if(NOT ARGN) + message(SEND_ERROR "Error: rrts_protobuf_generate_cpp() called without any proto files") + return() + endif() + + if(PROTOBUF_GENERATE_CPP_APPEND_PATH) + # Create an include path for each file specified + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(abs_path ${abs_fil} PATH) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + else() + set(_protoc_include -I ${CMAKE_CURRENT_SOURCE_DIR}) + endif() + + if(DEFINED PROTOBUF_IMPORT_DIRS) + foreach(dir ${PROTOBUF_IMPORT_DIRS}) + get_filename_component(abs_path ${dir} ABSOLUTE) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + endif() + + set(${proto_srcs}) + set(${proto_hdrs}) + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(fil_we ${fil} NAME_WE) + + list(APPEND ${proto_srcs} "${output_dir}/${fil_we}.pb.cc") + list(APPEND ${proto_hdrs} "${output_dir}/${fil_we}.pb.h") + + add_custom_command( + OUTPUT "${output_dir}/${fil_we}.pb.cpp" + "${output_dir}/${fil_we}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${output_dir} ${_protoc_include} ${abs_fil} + DEPENDS ${abs_fil} + COMMENT "Running C++ protocol buffer compiler on ${fil}" + VERBATIM ) + endforeach() + + set_source_files_properties(${${proto_srcs}} ${${proto_hdrs}} PROPERTIES GENERATED TRUE) + set(${proto_srcs} ${${proto_srcs}} PARENT_SCOPE) + set(${proto_hdrs} ${${proto_hdrs}} PARENT_SCOPE) +endfunction() diff --git a/roborts_detection/package.xml b/roborts_detection/package.xml new file mode 100755 index 00000000..dad19143 --- /dev/null +++ b/roborts_detection/package.xml @@ -0,0 +1,31 @@ + + roborts_detection + 1.0.0 + + The roborts_detection pakcage provides object detection for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + roscpp + rospy + roborts_common + roborts_msgs + roborts_camera + + actionlib + roscpp + rospy + roborts_common + roborts_msgs + roborts_camera + + + + + diff --git a/roborts_detection/util/CMakeLists.txt b/roborts_detection/util/CMakeLists.txt new file mode 100644 index 00000000..65fec016 --- /dev/null +++ b/roborts_detection/util/CMakeLists.txt @@ -0,0 +1,11 @@ +project(detection_util) + +add_library(cv_toolbox INTERFACE) +target_sources(cv_toolbox INTERFACE "${CMAKE_CURRENT_SOURCE_DIR}") +target_link_libraries(cv_toolbox INTERFACE ${catkin_LIBRARIES} + ) +target_include_directories(cv_toolbox + INTERFACE + ${catkin_INCLUDE_DIRS} + ) +add_library(detection::tool ALIAS cv_toolbox) \ No newline at end of file diff --git a/roborts_detection/util/cv_toolbox.h b/roborts_detection/util/cv_toolbox.h new file mode 100644 index 00000000..40afe355 --- /dev/null +++ b/roborts_detection/util/cv_toolbox.h @@ -0,0 +1,305 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_DETECTION_CVTOOLBOX_H +#define ROBORTS_DETECTION_CVTOOLBOX_H + +#include +#include +#include +//opencv +#include +//ros +#include +#include + +namespace roborts_detection { + +/** + * This class is a toolbox for RoboMaster detection. + */ + +enum BufferState { + IDLE = 0, + WRITE, + READ +}; + +class CVToolbox { + public: + /** + * @brief The constructor of the CVToolbox. + */ + explicit CVToolbox(std::string camera_name, + unsigned int buffer_size = 3) : + get_img_info_(false) + { + + ros::NodeHandle nh(camera_name); + image_transport::ImageTransport it(nh); + + camera_sub_ = it.subscribeCamera("image_raw", + 20, + boost::bind(&CVToolbox::ImageCallback, this, _1,_2)); + + image_buffer_.resize(buffer_size); + buffer_state_.resize(buffer_size); + index_ = 0; + capture_time_ = -1; + for (int i = 0; i < buffer_state_.size(); ++i) { + buffer_state_[i] = BufferState::IDLE; + } + latest_index_ = -1; + + } + + int GetCameraHeight() { + if (!get_img_info_) { + ROS_WARN("Can not get camera height info, because the first frame data wasn't received"); + return -1; + } + return camera_info_.height; + } + + int GetCameraWidth() { + if (!get_img_info_) { + ROS_WARN("Can not get camera width info, because the first frame data wasn't received"); + return -1; + } + return camera_info_.width; + } + + int GetCameraMatrix(cv::Mat &k_matrix) { + if (!get_img_info_) { + ROS_WARN("Can not get camera matrix info, because the first frame data wasn't received"); + return -1; + } + k_matrix = cv::Mat(3, 3, CV_64F, camera_info_.K.data()).clone(); + return 0; + } + + int GetCameraDistortion(cv::Mat &distortion) { + if (!get_img_info_) { + ROS_WARN("Can not get camera distortion info, because the first frame data wasn't received"); + return -1; + } + distortion = cv::Mat(camera_info_.D.size(), 1, CV_64F, camera_info_.D.data()).clone(); + return 0; + } + + int GetWidthOffSet() { + if (!get_img_info_) { + ROS_WARN("Can not get camera width offset info, because the first frame data wasn't received"); + return -1; + } + return camera_info_.roi.x_offset; + } + + int GetHeightOffSet() { + if (!get_img_info_) { + ROS_WARN("Can not get camera height offset info, because the first frame data wasn't received"); + return -1; + } + return camera_info_.roi.y_offset; + } + + void ImageCallback(const sensor_msgs::ImageConstPtr &img_msg, const sensor_msgs::CameraInfoConstPtr &camera_info_msg) { + if(!get_img_info_){ + camera_info_ = *camera_info_msg; + capture_begin_ = std::chrono::high_resolution_clock::now(); + get_img_info_ = true; + } else { + capture_time_ = std::chrono::duration>(std::chrono::high_resolution_clock::now() - capture_begin_).count(); + capture_begin_ = std::chrono::high_resolution_clock::now(); +// ROS_WARN("capture time: %lf", capture_time_); + } + capture_begin_ = std::chrono::high_resolution_clock::now(); + auto write_begin = std::chrono::high_resolution_clock::now(); + for (int i = 0; i < buffer_state_.size(); ++i) { + if (buffer_state_[i] != BufferState::READ) { + image_buffer_[i] = cv_bridge::toCvShare(img_msg, "bgr8")->image.clone(); + buffer_state_[i] = BufferState::WRITE; + lock_.lock(); + latest_index_ = i; + lock_.unlock(); + } + } + /* + ROS_WARN("write time: %lf", std::chrono::duration> + (std::chrono::high_resolution_clock::now() - write_begin).count());*/ + } + /** + * @brief Get next new image. + * @param src_img Output image + */ + int NextImage(cv::Mat &src_img) { + if (latest_index_ < 0) { + ROS_WARN("Call image when no image received"); + return -1; + } + int temp_index = -1; + lock_.lock(); + if (buffer_state_[latest_index_] == BufferState::WRITE) { + buffer_state_[latest_index_] = BufferState::READ; + } else { + ROS_INFO("No image is available"); + lock_.unlock(); + return temp_index; + } + temp_index = latest_index_; + lock_.unlock(); + + src_img = image_buffer_[temp_index]; + return temp_index; + } + + void GetCaptureTime(double &capture_time) { + if (!get_img_info_) { + ROS_WARN("The first image doesn't receive"); + return; + } + if (capture_time_ < 0) { + ROS_WARN("The second image doesn't receive"); + return; + } + + capture_time = capture_time_; + } + + /** + * @brief Return the image after use + * @param return_index Index gets from function 'int NextImage(cv::Mat &src_img)' + */ + void ReadComplete(int return_index) { + + if (return_index < 0 || return_index > (buffer_state_.size() - 1)) { + ROS_ERROR("Return index error, please check the return_index"); + return; + } + + buffer_state_[return_index] = BufferState::IDLE; + } + + /** + * @brief Highlight the blue or red region of the image. + * @param image Input image ref + * @return Single channel image + */ + cv::Mat DistillationColor(const cv::Mat &src_img, unsigned int color, bool using_hsv) { + if(using_hsv) { + cv::Mat img_hsv; + cv::cvtColor(src_img, img_hsv, CV_BGR2HSV); + if (color == 0) { + cv::Mat img_hsv_blue, img_threshold_blue; + img_hsv_blue = img_hsv.clone(); + cv::Mat blue_low(cv::Scalar(90, 150, 46)); + cv::Mat blue_higher(cv::Scalar(140, 255, 255)); + cv::inRange(img_hsv_blue, blue_low, blue_higher, img_threshold_blue); + return img_threshold_blue; + } else { + cv::Mat img_hsv_red1, img_hsv_red2, img_threshold_red, img_threshold_red1, img_threshold_red2; + img_hsv_red1 = img_hsv.clone(); + img_hsv_red2 = img_hsv.clone(); + cv::Mat red1_low(cv::Scalar(0, 43, 46)); + cv::Mat red1_higher(cv::Scalar(3, 255, 255)); + + cv::Mat red2_low(cv::Scalar(170, 43, 46)); + cv::Mat red2_higher(cv::Scalar(180, 255, 255)); + cv::inRange(img_hsv_red1, red1_low, red1_higher, img_threshold_red1); + cv::inRange(img_hsv_red2, red2_low, red2_higher, img_threshold_red2); + img_threshold_red = img_threshold_red1 | img_threshold_red2; + //cv::imshow("img_threshold_red", img_threshold_red); + return img_threshold_red; + } + } else { + std::vector bgr; + cv::split(src_img, bgr); + if (color == 1) { + cv::Mat result_img; + cv::subtract(bgr[2], bgr[1], result_img); + return result_img; + } else if (color == 0) { + cv::Mat result_img; + cv::subtract(bgr[0], bgr[2], result_img); + return result_img; + } + } + } + /** + * @brief The wrapper for function cv::findContours + * @param binary binary image ref + * @return Contours that found. + */ + std::vector> FindContours(const cv::Mat &binary_img) { + std::vector> contours; + const auto mode = CV_RETR_EXTERNAL; + const auto method = CV_CHAIN_APPROX_SIMPLE; + cv::findContours(binary_img, contours, mode, method); + return contours; + } + /** + * @brief Draw rectangle. + * @param img The image will be drew on + * @param rect The target rectangle + * @param color Rectangle color + * @param thickness Thickness of the line + */ + void DrawRotatedRect(const cv::Mat &img, const cv::RotatedRect &rect, const cv::Scalar &color, int thickness) { + cv::Point2f vertex[4]; + + cv::Point2f center = rect.center; + float angle = rect.angle; + std::ostringstream ss; + ss << angle; + std::string text(ss.str()); + int font_face = cv::FONT_HERSHEY_COMPLEX; + double font_scale = 0.5; + cv::putText(img, text, center, font_face, font_scale, cv::Scalar(0, 255, 255), thickness, 8, 0); + + rect.points(vertex); + for (int i = 0; i < 4; i++) + cv::line(img, vertex[i], vertex[(i + 1) % 4], color, thickness); + } + + void DrawRotatedRect(const cv::Mat &img, const cv::RotatedRect &rect, const cv::Scalar &color, int thickness, float angle) { + cv::Point2f vertex[4]; + + cv::Point2f center = rect.center; + std::ostringstream ss; + ss << (int)(angle); + std::string text(ss.str()); + int font_face = cv::FONT_HERSHEY_COMPLEX; + double font_scale = 0.5; + cv::putText(img, text, center, font_face, font_scale, cv::Scalar(0, 100, 0), thickness, 8, 0); + + rect.points(vertex); + for (int i = 0; i < 4; i++) + cv::line(img, vertex[i], vertex[(i + 1) % 4], color, thickness); + } + private: + std::vector image_buffer_; + std::vector buffer_state_; + int latest_index_; + std::mutex lock_; + int index_; + std::chrono::high_resolution_clock::time_point capture_begin_; + double capture_time_; + + image_transport::CameraSubscriber camera_sub_; + bool get_img_info_; + sensor_msgs::CameraInfo camera_info_; +}; +} //namespace roborts_detection + +#endif //ROBORTS_DETECTION_CVTOOLBOX_H diff --git a/roborts_localization/CMakeLists.txt b/roborts_localization/CMakeLists.txt new file mode 100755 index 00000000..d225dbc9 --- /dev/null +++ b/roborts_localization/CMakeLists.txt @@ -0,0 +1,43 @@ +project(roborts_localization) +cmake_minimum_required(VERSION 3.1) + +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_CXX_STANDARD 14) + +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) + +find_package(Threads REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Glog REQUIRED) +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + nav_msgs + message_filters + sensor_msgs + ) +catkin_package() + +add_subdirectory(amcl) + +include_directories( + src +) + +add_executable( + localization_node + localization_node.cpp + localization_node.h + localization_math.cpp + ) + +target_link_libraries(localization_node + PUBLIC + localization::amcl + ${EIGEN3_LIBRARIES} + ${catkin_LIBRARIES} + ${GLOG_LIBRARY} + ) + +target_include_directories(localization_node PUBLIC + ${catkin_INCLUDE_DIRS}) diff --git a/roborts_localization/amcl/CMakeLists.txt b/roborts_localization/amcl/CMakeLists.txt new file mode 100755 index 00000000..02c8de6b --- /dev/null +++ b/roborts_localization/amcl/CMakeLists.txt @@ -0,0 +1,37 @@ +project(amcl) + +#find_package(catkin REQUIRED COMPONENTS +# roscpp +# tf +# nav_msgs +# message_filters +# sensor_msgs +# ) + +add_library(amcl + amcl.cpp + amcl.h + amcl_config.h + ${CMAKE_CURRENT_SOURCE_DIR}/map/amcl_map.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter_gaussian_pdf.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter_kdtree.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter_sample.h + ${CMAKE_CURRENT_SOURCE_DIR}/sensors/sensor_laser.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/sensors/sensor_odom.cpp) + +target_include_directories(${PROJECT_NAME} + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/../ + ${EIGEN3_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + ${EIGEN3_LIBRARIES} + ${catkin_LIBRARIES} + ${GLOG_LIBRARY} + ) + +add_library(localization::amcl ALIAS amcl) diff --git a/roborts_localization/amcl/amcl.cpp b/roborts_localization/amcl/amcl.cpp new file mode 100755 index 00000000..11e14603 --- /dev/null +++ b/roborts_localization/amcl/amcl.cpp @@ -0,0 +1,539 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include +#include "amcl.h" + +namespace roborts_localization { + +std::vector > Amcl::free_space_indices; + +void Amcl::Init(const Vec3d &init_pose, const Vec3d &init_cov) { + + d_thresh_ = amcl_param_.update_min_d; + a_thresh_ = amcl_param_.update_min_a; + max_uwb_particles_ = amcl_param_.max_uwb_particles; + resample_uwb_factor_ = amcl_param_.resample_uwb_factor; + + CHECK_GT(amcl_param_.max_particles, amcl_param_.max_uwb_particles) + << "UWB max correcting particles number must be less than max sample particles"; + uwb_cov_x_ = amcl_param_.uwb_cov_x; + uwb_cov_y_ = amcl_param_.uwb_cov_y; + + use_global_localization_ = amcl_param_.use_global_localization; + random_heading_ = amcl_param_.random_heading; + + UpdatePoseFromParam(init_pose, init_cov); + + cloud_pub_interval_.fromSec(1.0); + + LOG_INFO << "Amcl Init!"; + +} + +Amcl::~Amcl() { + Reset(); + LOG_INFO << "Delete Amcl"; +} + +void Amcl::GetParamFromRos(ros::NodeHandle *nh) { + amcl_param_.GetParam(nh); + CHECK_GT(amcl_param_.laser_likelihood_max_dist, 0); +} + +void Amcl::Reset() { + if (map_ptr_ != nullptr) { + map_ptr_.reset(); + } + if (pf_ptr_ != nullptr) { + pf_ptr_.reset(); + } +} + +void Amcl::HandleMapMessage(const nav_msgs::OccupancyGrid &map_msg, const Vec3d &init_pose, const Vec3d &init_cov) { + Reset(); + map_ptr_.reset(new AmclMap()); + map_ptr_->ConvertFromMsg(map_msg); + + // Index of free space + Amcl::free_space_indices.resize(0); + for (int i = 0; i < map_ptr_->GetSizeX(); i++) { + for (int j = 0; j < map_ptr_->GetSizeY(); j++) { + if (map_ptr_->CheckIndexFree(i, j)) { + this->free_space_indices.push_back(std::make_pair(i, j)); + } + } + } + + pf_ptr_.reset(new ParticleFilter(amcl_param_.min_particles, + amcl_param_.max_particles, + amcl_param_.recovery_alpha_slow, + amcl_param_.recovery_alpha_fast, + std::bind(&Amcl::UniformPoseGenerator, this), + map_ptr_)); + pf_ptr_->SetKldParam(amcl_param_.kld_err, amcl_param_.kld_z); + + UpdatePoseFromParam(init_pose, init_cov); + + Vec3d pf_init_pose_mean; + Mat3d pf_init_pose_cov; + pf_init_pose_cov.setZero(); + pf_init_pose_mean.setZero(); + pf_init_pose_mean = init_pose_; + pf_init_pose_cov(0, 0) = init_cov_(0); + pf_init_pose_cov(1, 1) = init_cov_(1); + pf_init_pose_cov(2, 2) = init_cov_(2); + pf_ptr_->InitByGuassian(pf_init_pose_mean, pf_init_pose_cov); + pf_init_ = false; + + odom_model_ptr_ = std::make_unique(amcl_param_.odom_alpha1, + amcl_param_.odom_alpha2, + amcl_param_.odom_alpha3, + amcl_param_.odom_alpha4, + amcl_param_.odom_alpha5); + + laser_model_ptr_ = std::make_unique(amcl_param_.laser_max_beams, + map_ptr_); + LOG_INFO << "Initializing likelihood field model( this can take some time on large maps)"; + laser_model_ptr_->SetModelLikelihoodFieldProb + (amcl_param_.z_hit, + amcl_param_.z_rand, + amcl_param_.sigma_hit, + amcl_param_.laser_likelihood_max_dist, + amcl_param_.do_beamskip, + amcl_param_.beam_skip_distance, + amcl_param_.beam_skip_threshold, + amcl_param_.beam_skip_error_threshold, + amcl_param_.laser_filter_weight + ); + LOG_INFO << "Done initializing likelihood field model."; + + if (use_global_localization_) { + GlobalLocalization(); + } else { + if (random_heading_) { + RandomHeadingGlobalLocalization(); + } else { + ApplyInitPose(); + } + } + +} + +const nav_msgs::OccupancyGrid &Amcl::GetDistanceMapMsg() { + return map_ptr_->ConvertDistanMaptoMapMsg(); +} + +void Amcl::UpdatePoseFromParam(const Vec3d &init_pose, const Vec3d &init_cov) { + init_pose_[0] = 0.0; + init_pose_[1] = 0.0; + init_pose_[2] = 0.0; + init_cov_[0] = 0.5 * 0.5; + init_cov_[1] = 0.5 * 0.5; + init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0); + + if (!std::isnan(init_pose[0])) { + init_pose_[0] = init_pose[0]; + } else { + LOG_WARNING << "ignoring NAN in initial pose X position"; + } + if (!std::isnan(init_pose[1])) { + init_pose_[1] = init_pose[1]; + } else { + LOG_WARNING << "ignoring NAN in initial pose Y position"; + } + if (!std::isnan(init_pose[2])) { + init_pose_[2] = init_pose[2]; + } else { + LOG_WARNING << "ignoring NAN in initial pose Yaw"; + } + if (!std::isnan(init_cov[0])) { + init_cov_[0] = init_cov[0]; + } else { + LOG_WARNING << "ignoring NAN in initial covariance XX"; + } + if (!std::isnan(init_cov[1])) { + init_cov_[1] = init_cov[1]; + } else { + LOG_WARNING << "ignoring NAN in initial covariance YY"; + } + if (!std::isnan(init_cov[2])) { + init_cov_[1] = init_cov[2]; + } else { + LOG_WARNING << "ignoring NAN in initial covariance AA"; + } + + DLOG_INFO << "Updated init pose " << init_pose_; +} + +void Amcl::HandleInitialPoseMessage(Vec3d pf_init_pose_mean, + Mat3d pf_init_pose_cov) { + initial_pose_hyp_.reset(new AmclHyp()); + initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; + initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; + ApplyInitPose(); +} + +void Amcl::ApplyInitPose() { + if (initial_pose_hyp_ != nullptr && map_ptr_ != nullptr) { + pf_ptr_->InitByGuassian(initial_pose_hyp_->pf_pose_mean, + initial_pose_hyp_->pf_pose_cov); + pf_init_ = false; + initial_pose_hyp_.reset(); + } +} + +void Amcl::SetLaserSensorPose(Vec3d laser_pose) { + laser_update_ = true; + laser_model_ptr_->SetLaserPose(laser_pose); +} + +Vec3d Amcl::UniformPoseGenerator() { + auto rand_index = static_cast(drand48() * free_space_indices.size()); + std::pair free_point = free_space_indices[rand_index]; + Vec3d p; + map_ptr_->ConvertMapCoordsToWorldCoords(free_point.first, free_point.second, p[0], p[1]); + p[2] = drand48() * 2 * M_PI - M_PI; + return p; +} + +bool Amcl::GlobalLocalization() { + if (map_ptr_ == nullptr) { + return true; + } + + LOG_INFO << "Initializing with uniform distribution"; + PfInitModelFunc UniformPoseGeneratorFunc = std::bind(&Amcl::UniformPoseGenerator, this); + pf_ptr_->InitByModel(UniformPoseGeneratorFunc); + + LOG_INFO << "Global initialization done!"; + pf_init_ = false; + return true; +} + +bool Amcl::RandomHeadingGlobalLocalization() { + if (map_ptr_ != nullptr) { + Mat3d pf_init_pose_cov; + pf_init_pose_cov.setZero(); + pf_init_pose_cov(0, 0) = init_cov_(0); + pf_init_pose_cov(1, 1) = init_cov_(1); + pf_init_pose_cov(2, 2) = init_cov_(2); + + pf_ptr_->InitByGuassianWithRandomHeading(init_pose_, + pf_init_pose_cov); + pf_init_ = false; + return true; + } + return false; +} + +void Amcl::UpdateUwb(const Vec3d &uwb_pose, const Vec3d &uwb_cov) { + uwb_cov_x_ = uwb_cov[0]; + uwb_cov_y_ = uwb_cov[1]; + UpdateUwb(uwb_pose); +} + +void Amcl::UpdateUwb(const Vec3d &uwb_pose) { + + std::lock_guard sample_lock(mutex_); + + auto set_ptr = pf_ptr_->GetCurrentSampleSetPtr(); + double total = 0; + std::vector distance; + distance.resize(set_ptr->sample_count, 0); + + for (int i = 0; i < set_ptr->sample_count; i++) { + distance[i] = math::EuclideanDistance(uwb_pose[0], + uwb_pose[1], + set_ptr->samples_vec[i].pose[0], + set_ptr->samples_vec[i].pose[1]); + if (distance[i] > (map_ptr_->GetDiagDistance() / resample_uwb_factor_)) { + set_ptr->samples_vec[i].pose[0] = uwb_pose[0] + math::RandomGaussianNumByStdDev(uwb_cov_x_); + set_ptr->samples_vec[i].pose[1] = uwb_pose[1] + math::RandomGaussianNumByStdDev(uwb_cov_y_); + set_ptr->samples_vec[i].pose[2] = uwb_pose[2] + math::RandomGaussianNumByStdDev((M_PI / 3.0) * (M_PI / 3.0)); + distance[i] = math::EuclideanDistance(uwb_pose[0], + uwb_pose[1], + set_ptr->samples_vec[i].pose[0], + set_ptr->samples_vec[i].pose[1]); + } + total += distance[i]; + } + + CHECK_GT(total, 0); + for (int j = 0; j < set_ptr->sample_count; j++) { + set_ptr->samples_vec[j].weight *= (total - distance[j]) / total; + } + + std::sort(set_ptr->samples_vec.begin(), + set_ptr->samples_vec.begin() + set_ptr->sample_count, + [](const ParticleFilterSample &sample1, const ParticleFilterSample &sample2) { + return sample1.weight < sample2.weight; + }); + + auto least_weight_particle_num = static_cast(max_uwb_particles_); + if (least_weight_particle_num <= 0) { + least_weight_particle_num = 1; + } + for (int i = 0; i < least_weight_particle_num; i++) { + set_ptr->samples_vec[i].pose[0] = uwb_pose[0] + math::RandomGaussianNumByStdDev(uwb_cov_x_); + set_ptr->samples_vec[i].pose[1] = uwb_pose[1] + math::RandomGaussianNumByStdDev(uwb_cov_y_); + set_ptr->samples_vec[i].pose[2] = + set_ptr->samples_vec[i].pose[2] + math::RandomGaussianNumByStdDev((M_PI / 12.0) * (M_PI / 12.0)); + } + +} + +int Amcl::Update(const Vec3d &pose, + const sensor_msgs::LaserScan &laser_scan, + const double &angle_min, + const double &angle_increment, + geometry_msgs::PoseArray &particle_cloud_pose_msg, + HypPose &hyp_pose) { + + std::lock_guard sample_lock(mutex_); + + publish_pose_ = false; + publish_particle_pose_cloud_ = false; + update_tf_ = false; + + UpdateOdomPoseData(pose); + + resampled_ = false; + if (laser_update_) { + UpdateLaser(laser_scan, angle_min, angle_increment, pose, particle_cloud_pose_msg); + } + UpdateFilter(hyp_pose, laser_scan.header.stamp); +}; + +void Amcl::UpdateOdomPoseData(Vec3d pose) { + Vec3d delta; + delta.setZero(); + + if (pf_init_) { + + // Compute change in pose + delta[0] = pose[0] - pf_odom_pose_[0]; + delta[1] = pose[1] - pf_odom_pose_[1]; + delta[2] = math::AngleDiff(pose[2], pf_odom_pose_[2]); + + // See if we should update the filter + bool update = std::fabs(delta[0]) > d_thresh_ || + std::fabs(delta[1]) > d_thresh_ || + std::fabs(delta[2]) > a_thresh_; + + //TODO +// update = true; + + // Set the laser update flags + if (update) { + laser_update_ = true; + } + } + + force_publication_ = false; + if (!pf_init_) { + // Pose at last filter update + pf_odom_pose_ = pose; + // Filter is now initialized" + pf_init_ = true; + // Set update sensor data flag + laser_update_ = true; + force_publication_ = true; + resample_count_ = 0; + } // If the robot has moved, update the filter + else if (pf_init_ && laser_update_) { + DLOG_INFO << "Robot has moved, update the filter"; + SensorOdomData odom_data; + odom_data.pose = pose; + odom_data.delta = delta; + odom_model_ptr_->UpdateAction(pf_ptr_->GetCurrentSampleSetPtr(), odom_data); + } +} + +void Amcl::UpdateLaser(const sensor_msgs::LaserScan &laser_scan, + double angle_min, + double angle_increment, + const Vec3d &pose, + geometry_msgs::PoseArray &particle_cloud_pose_msg) { + + SensorLaserData laser_data; + laser_data.range_count = laser_scan.ranges.size(); + + // Apply range min/max thresholds, if the user supplied them + if (laser_max_range_ > 0.0) { + laser_data.range_max = std::min(laser_scan.range_max, + static_cast(laser_max_range_)); + } else { + laser_data.range_max = laser_scan.range_max; + } + + double range_min; + if (laser_min_range_ > 0.0) { + range_min = std::max(laser_scan.range_min, static_cast(laser_min_range_)); + } else { + range_min = laser_scan.range_min; + } + + laser_data.ranges_mat.resize(laser_data.range_count, 2); + laser_data.ranges_mat.setZero(); + + for (int i = 0; i < laser_data.range_count; i++) { + // amcl doesn't (yet) have a concept of min range. So we'll map short + // readings to max range. + if (laser_scan.ranges[i] <= range_min) { + laser_data.ranges_mat(i, 0) = laser_data.range_max; + } else { + laser_data.ranges_mat(i, 0) = laser_scan.ranges[i]; + } + // Compute bearing + laser_data.ranges_mat(i, 1) = angle_min + (i * angle_increment); + } + + total_weight_ = laser_model_ptr_->UpdateSensor(pf_ptr_, &laser_data); + pf_ptr_->UpdateOmega(total_weight_); + + laser_update_ = false; + + pf_odom_pose_ = pose; + + particle_cloud_pose_msg = ResampleParticles(); + +} + +geometry_msgs::PoseArray Amcl::ResampleParticles() { + + geometry_msgs::PoseArray particle_cloud_pose_msg; + + if (!(++resample_count_ % resample_interval_)) { + DLOG_INFO << "Resample the particles"; + pf_ptr_->UpdateResample(); + resampled_ = true; + } + + auto set_ptr = pf_ptr_->GetCurrentSampleSetPtr(); + DLOG_INFO << "Number of samples : " << set_ptr->sample_count; + + // Publish the resulting particle cloud + particle_cloud_pose_msg.poses.resize(set_ptr->sample_count); + for (int i = 0; i < set_ptr->sample_count; i++) { + tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set_ptr->samples_vec[i].pose[2]), + tf::Vector3(set_ptr->samples_vec[i].pose[0], + set_ptr->samples_vec[i].pose[1], + 0)), + particle_cloud_pose_msg.poses[i]); + } + publish_particle_pose_cloud_ = true; + return particle_cloud_pose_msg; +} + +void Amcl::UpdateFilter(HypPose &hyp_pose, + ros::Time laser_msg_timestamp) { + + if (resampled_ || force_publication_) { + if (!resampled_) { + DLOG_INFO << "Recompute particle filter cluster statistics"; + pf_ptr_->ClusterStatistics(); + } + // Read out the current hypotheses + double max_weight = 0.0; + int max_weight_hyp = -1; + std::vector hyps; + hyps.resize(pf_ptr_->GetCurrentSampleSetPtr()->cluster_count); + for (int hyp_count = 0; + hyp_count < pf_ptr_->GetCurrentSampleSetPtr()->cluster_count; + hyp_count++) { + double weight; + Vec3d pose_mean; + Mat3d pose_cov; + + if (!pf_ptr_->GetClusterStatistics(hyp_count, + &weight, + &pose_mean, + &pose_cov)) { + LOG_ERROR << "Couldn't get stats on cluster " << hyp_count; + break; + } + + hyps[hyp_count].weight = weight; + hyps[hyp_count].pf_pose_mean = pose_mean; + hyps[hyp_count].pf_pose_cov = pose_cov; + + if (hyps[hyp_count].weight > max_weight) { + max_weight = hyps[hyp_count].weight; + max_weight_hyp = hyp_count; + } + } + + if (max_weight > 0.0) { + + DLOG_INFO << "Max weight: " << max_weight + << ", Pose: " + << hyps[max_weight_hyp].pf_pose_mean[0] << ", " + << hyps[max_weight_hyp].pf_pose_mean[1] << ", " + << hyps[max_weight_hyp].pf_pose_mean[2]; + + auto set = pf_ptr_->GetCurrentSampleSetPtr(); + + hyp_pose.pose_mean = hyps[max_weight_hyp].pf_pose_mean; + hyp_pose.pose_set_cov = set->covariant; + + publish_pose_ = true; + update_tf_ = true; + } else { + LOG_ERROR << "Max weight of clusters less than 0!"; + publish_pose_ = false; + update_tf_ = false; + } + } else { + // Nothing changed, republish the last transform. + update_tf_ = false; + publish_pose_ = true; + + } +} + +bool Amcl::CheckPosePublish() { + return publish_pose_; +} +bool Amcl::CheckParticlePoseCloudPublish() { + return publish_particle_pose_cloud_; +} +bool Amcl::CheckTfUpdate() { + return update_tf_; +} + +}// roborts_localization \ No newline at end of file diff --git a/roborts_localization/amcl/amcl.h b/roborts_localization/amcl/amcl.h new file mode 100755 index 00000000..471a4443 --- /dev/null +++ b/roborts_localization/amcl/amcl.h @@ -0,0 +1,244 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + + +#ifndef ROBORTS_LOCALIZATION_AMCL_AMCL_H +#define ROBORTS_LOCALIZATION_AMCL_AMCL_H + +#include +#include + +#include +#include +#include +#include + +#include "log.h" +#include "localization_math.h" +#include "types.h" + +#include "map/amcl_map.h" +#include "particle_filter/particle_filter.h" +#include "sensors/sensor_laser.h" +#include "sensors/sensor_odom.h" +#include "amcl_config.h" + +namespace roborts_localization { +/** + * @brief Pose hypothesis type in localization module + */ +class HypPose { + public: + //! Mean of pose esimate + Vec3d pose_mean; + //! Covariance of pose estimate + Mat3d pose_set_cov; +}; + +/** + * @brief Pose hypothesis type in Amcl + */ +class AmclHyp { + public: + //! Total weight (weights sum to 1) + double weight; + //! Mean of pose esimate + Vec3d pf_pose_mean; + //! Covariance of pose estimate + Mat3d pf_pose_cov; +}; + +/** + * @brief Amcl algorithm wrapping from ROS-Navigation + */ +class Amcl { + public: + + /** + * @brief Amcl initialization function + */ + void Init(const Vec3d &init_pose, const Vec3d &init_cov); + + /** + * @brief Amcl destructor. + */ + ~Amcl(); + + void GetParamFromRos(ros::NodeHandle *nh); + + /** + * @brief Map message handler + * @param map_msg Static Map message + */ + void HandleMapMessage(const nav_msgs::OccupancyGrid &map_msg, const Vec3d &init_pose, const Vec3d &init_cov); + + /** + * @brief Initial pose estimation message handler + * @param pf_init_pose_mean Initial mean of pose estimation + * @param pf_init_pose_cov Initial covariance of pose estimation + */ + void HandleInitialPoseMessage(Vec3d pf_init_pose_mean, + Mat3d pf_init_pose_cov); + + /** + * @brief Set up laser pose in base frame + * @param laser_pose Laser pose in base frame + */ + void SetLaserSensorPose(Vec3d laser_pose); + + /** + * @brief Update AMCL algorithm + * @param pose Odom pose + * @param laser_scan Laser_scan msg + * @param angle_min Laser scan msg angle min in base frame + * @param angle_increment Laser scan msg angle increment in base frame + * @param particle_cloud_pose_msg Particle cloud pose msg to publish + * @param hyp_pose Pose hypothesis to publish + * @return Error code + */ + int Update(const Vec3d &pose, + const sensor_msgs::LaserScan &laser_scan, + const double &angle_min, + const double &angle_increment, + geometry_msgs::PoseArray &particle_cloud_pose_msg, + HypPose &hyp_pose + ); + + void UpdateUwb(const Vec3d &uwb_pose, const Vec3d &uwb_cov); + + void UpdateUwb(const Vec3d &uwb_pose); + + /** + * @brief Check amcl pose publish state + * @return Return true if publish amcl pose + */ + bool CheckPosePublish(); + + /** + * @brief Check particle cloud pose publish state + * @return Return true if publish particle cloud pose + */ + bool CheckParticlePoseCloudPublish(); + + /** + * @brief Check TF update state + * @return Return true if need update TF + */ + bool CheckTfUpdate(); + + bool RandomHeadingGlobalLocalization(); + + const nav_msgs::OccupancyGrid &GetDistanceMapMsg(); + + private: + void Reset(); + + bool GlobalLocalization(); + + void UpdatePoseFromParam(const Vec3d &init_pose, const Vec3d &init_cov); + void ApplyInitPose(); + + void UpdateOdomPoseData(Vec3d pose); + + void UpdateLaser(const sensor_msgs::LaserScan &laser_scan, + double angle_min, + double angle_increment, + const Vec3d &pose, + geometry_msgs::PoseArray &particle_cloud_pose_msg); + + void UpdateFilter(HypPose &hyp_pose, + ros::Time laser_msg_timestamp); + + geometry_msgs::PoseArray ResampleParticles(); + + Vec3d UniformPoseGenerator(); + + public: + AmclConfig amcl_param_; + + private: + std::mutex mutex_; + + int max_uwb_particles_ = 1; + double uwb_cov_x_ = 0.01; + double uwb_cov_y_ = 0.01; + double resample_uwb_factor_ = 3.0; + + double d_thresh_ = 0.2; + double a_thresh_ = M_PI / 6.0; + int resample_interval_ = 2; + int resample_count_ = 0; + double laser_min_range_ = 0; + double laser_max_range_ = 0; + double total_weight_ = 0; + + Vec3d init_pose_; + Vec3d init_cov_; + + std::unique_ptr initial_pose_hyp_ = nullptr; + + Vec3d pf_odom_pose_; + + std::shared_ptr map_ptr_; + + ParticleFilterPtr pf_ptr_; + bool pf_init_ = true; + + std::unique_ptr laser_model_ptr_; + std::unique_ptr odom_model_ptr_; + + bool use_global_localization_ = true; + bool random_heading_ = true; + + ros::Duration cloud_pub_interval_; + + static std::vector > free_space_indices; + + bool laser_update_ = true; + bool resampled_ = false; + bool force_publication_ = false; + + //Publish flag + bool publish_particle_pose_cloud_ = false; + bool publish_pose_ = false; + bool update_tf_ = false; + +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_AMCL_H diff --git a/roborts_localization/amcl/amcl_config.h b/roborts_localization/amcl/amcl_config.h new file mode 100644 index 00000000..f50cbdfc --- /dev/null +++ b/roborts_localization/amcl/amcl_config.h @@ -0,0 +1,100 @@ +#ifndef ROBORTS_LOCALIZATION_AMCL_CONFIG_H +#define ROBORTS_LOCALIZATION_AMCL_CONFIG_H + +#include + +namespace roborts_localization { + +struct AmclConfig { + void GetParam(ros::NodeHandle *nh) { + nh->param("use_map_topic", use_map_topic, true); + nh->param("first_map_only", use_map_topic, true); + nh->param("gui_publish_rate", gui_publish_rate, 10); + nh->param("laser_min_range", laser_min_range, 0.15); + nh->param("laser_max_range", laser_max_range, 8.0); + nh->param("laser_max_beams", laser_max_beams, 30); + nh->param("min_particles", min_particles, 500); + nh->param("max_particles", max_particles, 5000); + nh->param("kld_error", kld_err, 0.05); + nh->param("kld_z", kld_z, 0.99); + nh->param("z_hit", z_hit, 0.5); + nh->param("z_rand", z_rand, 0.5); + nh->param("sigma_hit", sigma_hit, 0.2); + nh->param("lambda_short", lambda_short, 0.1); + nh->param("laser_likelihood_max_dist", laser_likelihood_max_dist, 2.0); + nh->param("do_beamskip", do_beamskip, true); + nh->param("beam_skip_distance", beam_skip_distance, 0.5); + nh->param("beam_skip_threshold", beam_skip_threshold, 0.3); + nh->param("beam_skip_error_threshold", beam_skip_error_threshold, 0.9); + nh->param("odom_alpha1", odom_alpha1, 0.005); + nh->param("odom_alpha2", odom_alpha2, 0.005); + nh->param("odom_alpha3", odom_alpha3, 0.01); + nh->param("odom_alpha4", odom_alpha4, 0.005); + nh->param("odom_alpha5", odom_alpha5, 0.003); + nh->param("update_min_d", update_min_d, 0.2); + nh->param("update_min_a", update_min_a, 0.5); + nh->param("resample_interval", resample_interval, 1); + nh->param("transform_tolerance", transform_tolerance, 1); + nh->param("recovery_alpha_slow", recovery_alpha_slow, 0.1); + nh->param("recovery_alpha_fast", recovery_alpha_fast, 0.001); + nh->param("use_global_localization", use_global_localization, false); + nh->param("random_heading", random_heading, false); + nh->param("max_uwb_particles", max_uwb_particles, 10); + nh->param("uwb_cov_x", uwb_cov_x, 0.09); + nh->param("uwb_cov_y", uwb_cov_y, 0.09); + nh->param("resample_uwb_factor", resample_uwb_factor, 3.0); + }; + + bool use_map_topic; + bool first_map_only; + int gui_publish_rate; + + double laser_min_range; + double laser_max_range; + int laser_max_beams; + + int min_particles; + int max_particles; + + double kld_err; + double kld_z; + +// LaserModel laser_model = LASER_MODEL_LIKELIHOOD_FIELD_PROB; + double z_hit; + double z_rand; + double sigma_hit; + double lambda_short; + double laser_likelihood_max_dist; + bool do_beamskip; + double beam_skip_distance; + double beam_skip_threshold; + double beam_skip_error_threshold; + +// OdomModel odom_model = ODOM_MODEL_OMNI; + double odom_alpha1; + double odom_alpha2; + double odom_alpha3; + double odom_alpha4; + double odom_alpha5; + + double update_min_d; + double update_min_a; + + int resample_interval; + double transform_tolerance; + double recovery_alpha_slow; + double recovery_alpha_fast; + + bool use_global_localization; + bool random_heading; + double laser_filter_weight; + + int max_uwb_particles; + double uwb_cov_x; + double uwb_cov_y; + double resample_uwb_factor; +}; + +}// roborts_localization + +#endif // ROBORTS_LOCALIZATION_AMCL_CONFIG_H \ No newline at end of file diff --git a/roborts_localization/amcl/config/amcl.yaml b/roborts_localization/amcl/config/amcl.yaml new file mode 100755 index 00000000..e5834f9f --- /dev/null +++ b/roborts_localization/amcl/config/amcl.yaml @@ -0,0 +1,49 @@ +use_map_topic : true +first_map_only : false +gui_publish_rate : 10 + +laser_min_range : 0.15 +laser_max_range : 8.0 +laser_max_beams : 30 + +min_particles : 70 +max_particles : 5000 + +kld_err : 0.05 +kld_z : 0.99 + +laser_model : LASER_MODEL_LIKELIHOOD_FIELD_PROB +z_hit : 0.5 +z_rand : 0.5 +sigma_hit : 0.2 +lambda_short : 0.1 +laser_likelihood_max_dist : 5.0 +do_beamskip : false +beam_skip_distance : 0.5 +beam_skip_threshold : 0.3 +beam_skip_error_threshold : 0.9 + + +odom_model : ODOM_MODEL_OMNI +odom_alpha1 : 0.005 +odom_alpha2 : 0.005 +odom_alpha3 : 0.01 +odom_alpha4 : 0.005 +odom_alpha5 : 0.003 + +update_min_d : 0.05 +update_min_a : 0.03 + +resample_interval : 1 +transform_tolerance : 0.1 +recovery_alpha_slow : 0.001 +recovery_alpha_fast : 0.1 + +use_global_localization : false +random_heading : true +laser_filter_weight : 0.4 + +max_uwb_particles : 10 +uwb_cov_x : 0.06 +uwb_cov_y : 0.06 +resample_uwb_factor : 4.0 diff --git a/roborts_localization/amcl/map/amcl_map.cpp b/roborts_localization/amcl/map/amcl_map.cpp new file mode 100755 index 00000000..24bd1063 --- /dev/null +++ b/roborts_localization/amcl/map/amcl_map.cpp @@ -0,0 +1,262 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include "amcl_map.h" + +namespace roborts_localization { + +CachedDistanceMap::CachedDistanceMap(double scale, + double max_dist) { + scale_ = scale; + max_dist_ = max_dist; + cell_radius_ = static_cast(max_dist / scale); + distances_mat_.resize(cell_radius_ + 2); + for (auto it = distances_mat_.begin(); it != distances_mat_.end(); ++it) { + it->resize(cell_radius_ + 2, max_dist_); + } + + for (int i = 0; i < cell_radius_ + 2; i++) { + for (int j = 0; j < cell_radius_ + 2; j++) { + distances_mat_[i][j] = std::sqrt(i * i + j * j); + } + } +} + +AmclMap::~AmclMap() { + cells_vec_.clear(); + cells_vec_.shrink_to_fit(); +} + +int AmclMap::GetSizeX() const { + return size_x_; +} + +int AmclMap::GetSizeY() const { + return size_y_; +} + +double AmclMap::GetDiagDistance() const { + return diag_distance_; +} + +int AmclMap::ComputeCellIndexByMap(const int &i, const int &j) { + return i + j * this->size_x_; +}; + +void AmclMap::ConvertWorldCoordsToMapCoords(const double &x, + const double &y, + int &mx, + int &my) { + mx = (std::floor((x - this->origin_x_) / this->scale_ + 0.5) + this->size_x_ / 2); + my = (std::floor((y - this->origin_y_) / this->scale_ + 0.5) + this->size_y_ / 2); +} + +void AmclMap::ConvertMapCoordsToWorldCoords(const int &x, + const int &y, + double &wx, + double &wy) { + wx = (this->origin_x_ + ((x) - this->size_x_ / 2) * this->scale_); + wy = (this->origin_y_ + ((y) - this->size_y_ / 2) * this->scale_); +} + +bool AmclMap::CheckMapCoordsValid(const int &i, const int &j) { + return ((i >= 0) && (i < this->size_x_) && (j >= 0) && (j < this->size_y_)); +}; + +void AmclMap::ConvertFromMsg(const nav_msgs::OccupancyGrid &map_msg) { + this->size_x_ = map_msg.info.width; + this->size_y_ = map_msg.info.height; + this->scale_ = map_msg.info.resolution; + this->origin_x_ = map_msg.info.origin.position.x + (this->size_x_ / 2) * this->scale_; + this->origin_y_ = map_msg.info.origin.position.y + (this->size_y_ / 2) * this->scale_; + this->cells_vec_.resize(this->size_x_ * this->size_y_); + this->max_x_distance_ = static_cast(this->size_x_) * scale_; + this->max_y_distance_ = static_cast(this->size_y_) * scale_; + this->diag_distance_ = math::EuclideanDistance(0, 0, max_x_distance_, max_y_distance_); + LOG_INFO << "max x " << max_x_distance_ << " max y " << max_y_distance_; + + for (int i = 0; i < this->size_x_ * this->size_y_; i++) { + auto tmp_msg = static_cast(map_msg.data[i]); + if (tmp_msg == 0) { + this->cells_vec_[i].occ_state = -1; + } else if (tmp_msg == 100) { + this->cells_vec_[i].occ_state = +1; + } else { + this->cells_vec_[i].occ_state = 0; + } + } +} + +bool AmclMap::CheckIndexFree(int i, int j) { + if (this->cells_vec_[ComputeCellIndexByMap(i, j)].occ_state == -1) { + return true; + } else { + return false; + } +} + +const double &AmclMap::GetMaxOccDist() const { + return max_occ_dist_; +} + +const double &AmclMap::GetCellOccDistByIndex(int cell_index) { + return cells_vec_[cell_index].occ_dist; +} + +double AmclMap::GetCellOccDistByCoord(unsigned i, unsigned j) { + return GetCellOccDistByIndex(ComputeCellIndexByMap(i, j)); +} + +const nav_msgs::OccupancyGrid &AmclMap::ConvertDistanMaptoMapMsg() { + if (!distance_map_init_) { + distance_map_msg_.header.frame_id = "map"; + distance_map_msg_.info.width = this->size_x_; + distance_map_msg_.info.height = this->size_y_; + distance_map_msg_.info.resolution = this->scale_; + distance_map_msg_.info.origin.position.x = this->origin_x_ - (this->size_x_ / 2) * this->scale_; + distance_map_msg_.info.origin.position.y = this->origin_y_ - (this->size_y_ / 2) * this->scale_; + distance_map_msg_.data.resize(this->size_x_ * this->size_y_); + for (int i = 0; i < this->size_x_ * this->size_y_; i++) { + distance_map_msg_.data[i] = + (static_cast((cells_vec_[i].occ_dist) / (max_occ_dist_) * 100.0 / this->scale_)); + } + distance_map_init_ = true; + } + + return distance_map_msg_; +} + +void AmclMap::BuildDistanceMap(double scale, double max_dist) { + cached_distance_map_.reset(); + if (cached_distance_map_ == nullptr || cached_distance_map_->scale_ != scale || cached_distance_map_->max_dist_) { + if (cached_distance_map_ != nullptr) { + cached_distance_map_.reset(); + } + cached_distance_map_ = std::make_unique(scale, max_dist); + } +} + +void AmclMap::UpdateCSpace(double max_occ_dist) { + + mark_vec_ = std::make_unique>(); + mark_vec_->resize(this->size_x_ * this->size_y_); + CellDataPriorityQueue Q; + this->max_occ_dist_ = max_occ_dist; + BuildDistanceMap(this->scale_, this->max_occ_dist_); + + // Enqueue all the obstacle cells + CellData cell(0, + 0, + 0, + 0, + std::bind(&AmclMap::GetCellOccDistByCoord, this, std::placeholders::_1, std::placeholders::_2)); + for (int i = 0; i < this->size_x_; i++) { + cell.src_i_ = cell.i_ = i; + for (int j = 0; j < size_y_; j++) { + auto map_index_tmp = ComputeCellIndexByMap(i, j); + if (this->cells_vec_[map_index_tmp].occ_state == +1) { +// cell.occ_dist_ = this->cells_vec_[map_index_tmp].occ_dist = 0.0; + this->cells_vec_[map_index_tmp].occ_dist = 0.0; + cell.src_j_ = cell.j_ = j; + mark_vec_->at(map_index_tmp) = 1; + Q.push(cell); + } else { +// cell.occ_dist_ = this->cells_vec_[map_index_tmp].occ_dist = max_occ_dist; + this->cells_vec_[map_index_tmp].occ_dist = max_occ_dist; + } + } + } + + while (!Q.empty()) { + CellData current_cell_data = Q.top(); + if (current_cell_data.i_ > 0) { + Enqueue(current_cell_data.i_ - 1, current_cell_data.j_, + current_cell_data.src_i_, current_cell_data.src_j_, + Q); + } + if (current_cell_data.j_ > 0) { + Enqueue(current_cell_data.i_, current_cell_data.j_ - 1, + current_cell_data.src_i_, current_cell_data.src_j_, + Q); + } + if (current_cell_data.i_ < this->size_x_ - 1) { + Enqueue(current_cell_data.i_ + 1, current_cell_data.j_, + current_cell_data.src_i_, current_cell_data.src_j_, + Q); + } + if (current_cell_data.j_ < this->size_y_ - 1) { + Enqueue(current_cell_data.i_, current_cell_data.j_ + 1, + current_cell_data.src_i_, current_cell_data.src_j_, + Q); + } + Q.pop(); + } + + cached_distance_map_.reset(); + mark_vec_.reset(); +} + +void AmclMap::Enqueue(int i, int j, int src_i, int src_j, CellDataPriorityQueue &Q) { + + auto index = ComputeCellIndexByMap(i, j); + if (mark_vec_->at(index)) { + return; + } + + int di = std::abs(i - src_i); + int dj = std::abs(j - src_j); + double distance = cached_distance_map_->distances_mat_[di][dj]; + + if (distance > cached_distance_map_->cell_radius_) { + return; + } + + this->cells_vec_[index].occ_dist = distance * this->scale_; + + CellData cell_data(static_cast(i), + static_cast(j), + static_cast(src_i), + static_cast(src_j), + std::bind(&AmclMap::GetCellOccDistByCoord, this, std::placeholders::_1, std::placeholders::_2)); + Q.push(cell_data); + mark_vec_->at(index) = 1; + +} + +}// roborts_localization + diff --git a/roborts_localization/amcl/map/amcl_map.h b/roborts_localization/amcl/map/amcl_map.h new file mode 100755 index 00000000..6fa0820a --- /dev/null +++ b/roborts_localization/amcl/map/amcl_map.h @@ -0,0 +1,245 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_AMCL_MAP_AMCL_MAP_H +#define ROBORTS_LOCALIZATION_AMCL_MAP_AMCL_MAP_H + +#include +#include +#include +#include +#include + +#include "log.h" +#include "localization_math.h" + +namespace roborts_localization { + +class AmclMap; + +class CellData { + public: + CellData(unsigned int i, + unsigned int j, + unsigned int src_i, + unsigned int src_j, + const std::function &GetOccDistFunc) { + i_ = i; + j_ = j; + src_i_ = src_i; + src_j_ = src_j; + GetOccDist = GetOccDistFunc; + } + + public: + std::function GetOccDist; + unsigned int i_ = 0; + unsigned int j_ = 0; + unsigned int src_i_ = 0; + unsigned int src_j_ = 0; +}; + +class Cell { + public: + //! Occupancy state (-1 = free, 0 = unknown, +1 = occ) + int occ_state = 0; + + //! Distance to the nearest occupied cell + double occ_dist = 0; + +}; + +class CachedDistanceMap { + public: + CachedDistanceMap(double scale, double max_dist); + public: + double scale_ = 0, max_dist_ = 0; + int cell_radius_ = 0; + std::vector> distances_mat_; +}; + +struct CompareByOccDist { + bool operator()(const CellData &a, const CellData &b) { + return a.GetOccDist(a.i_, a.j_) > b.GetOccDist(b.i_, b.j_); + }; +}; +using CellDataPriorityQueue = std::priority_queue, CompareByOccDist>; + +class AmclMap { + public: + + virtual ~AmclMap(); + + /** + * @brief Convert static map message to AmclMap + * @param map_msg Static map message + */ + void ConvertFromMsg(const nav_msgs::OccupancyGrid &map_msg); + + /** + * @brief Update the cspace distance values, used by LikelihoodFiledProbe model + * @param max_occ_dist Max distance at which we care about obstacles + */ + void UpdateCSpace(double max_occ_dist); + + /** + * @brief Get max distance at which we care about obstacles + * @return Max distance at which we care about obstacles + */ + const double &GetMaxOccDist() const; + + const double &GetCellOccDistByIndex(int cell_index); + + double GetCellOccDistByCoord(unsigned i, unsigned j); + + /** + * @brief Compute cell index by map coords + * @param i Map coords i + * @param j Map coords j + * @return Returns cell index + */ + int ComputeCellIndexByMap(const int &i, const int &j); + + /** + * @brief Convert world coords to map coords + * @param x World coords x + * @param y World coords y + * @param mx Map coords x + * @param my Map coords y + */ + void ConvertWorldCoordsToMapCoords(const double &x, const double &y, int &mx, int &my); + + /** + * @brief Convert world coords to map coords + * @param x Map coords x + * @param y Map coords y + * @param wx World coords x + * @param wy World coords y + */ + void ConvertMapCoordsToWorldCoords(const int &x, const int &y, + double &wx, double &wy); + + /** + * @brief Check map coords valid + * @param i Map coords i + * @param j Map coords j + * @return + */ + bool CheckMapCoordsValid(const int &i, const int &j); + + /** + * @brief Check cell is free (occ_state == -1) + * @param i Map coords i + * @param j Map coords j + * @return + */ + bool CheckIndexFree(int i, int j); + + /** + * @brief Get map size x + * @return Returns map size x + */ + int GetSizeX() const; + + /** + * @brief Get map size y + * @return Returns map size y + */ + int GetSizeY() const; + + double GetDiagDistance() const; + + const nav_msgs::OccupancyGrid &ConvertDistanMaptoMapMsg(); + + private: + + void Enqueue(int i, int j, int src_i, int src_j, CellDataPriorityQueue &Q); + + void BuildDistanceMap(double scale, double max_dist); + + private: + + /** + * @brief The map data, stored as a grid + */ + std::vector cells_vec_; + + /** + * @brief The static distance map data + */ + std::unique_ptr cached_distance_map_; + + + std::unique_ptr> mark_vec_; + + /** + * @brief The static distance map messages data + */ + nav_msgs::OccupancyGrid distance_map_msg_; + + /** + * @brief Map origin; the map is a viewport onto a conceptual larger map. + */ + double origin_x_ = 0, origin_y_ = 0; + + double max_x_distance_, max_y_distance_ = 0, diag_distance_ = 0; + + /** + * @brief Map scale (m/cell) + */ + double scale_ = 0; + + /** + * @brief Map dimensions (number of cells) + */ + int size_x_ = 0, size_y_ = 0; + + /** + * @brief Max distance at which we care about obstacles, for constructing likelihood field. + */ + double max_occ_dist_ = 0; + + bool distance_map_init_ = false; + +}; + +using AmclMapPtr = std::shared_ptr; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_MAP_AMCL_MAP_H diff --git a/roborts_localization/amcl/particle_filter/particle_filter.cpp b/roborts_localization/amcl/particle_filter/particle_filter.cpp new file mode 100755 index 00000000..9f05a4b9 --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter.cpp @@ -0,0 +1,505 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free sof#include "sensors/sensor_laser.h"tware; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + + + +#include "particle_filter.h" +#include "particle_filter_gaussian_pdf.h" +#include "sensors/sensor_laser.h" + +namespace roborts_localization { + +ParticleFilter::ParticleFilter(int min_samples, + int max_samples, + double alpha_slow, + double alpha_fast, + const PfInitModelFunc &random_pose_func, + const std::shared_ptr &map_ptr) { + min_samples_ = min_samples; + max_samples_ = max_samples; + alpha_slow_ = alpha_slow; + alpha_fast_ = alpha_fast; + pop_err_ = 0.01; + pop_z_ = 3; + dist_threshold_ = 0.5; + current_set_ = 0; + + random_pose_func_ = random_pose_func; + map_ptr_ = map_ptr; + + for (auto &sample_set_it : sample_set_ptr_array_) { + sample_set_it = std::make_shared(); + sample_set_it->sample_count = max_samples; + sample_set_it->samples_vec.resize(max_samples); + for (int i = 0; i < sample_set_it->sample_count; i++) { + sample_set_it->samples_vec[i].weight = 1.0 / max_samples; + } + + sample_set_it->kd_tree_ptr = std::make_unique(); + sample_set_it->kd_tree_ptr->InitializeByMaxSize(3 * max_samples); + sample_set_it->cluster_count = 0; + sample_set_it->cluster_max_count = max_samples_; + sample_set_it->clusters_vec.resize(sample_set_it->cluster_max_count); + sample_set_it->mean.setZero(); + sample_set_it->covariant.setZero(); + } + + w_slow_ = 0; + w_fast_ = 0; + + alpha_slow_ = alpha_slow; + alpha_fast_ = alpha_fast; + + InitConverged(); + +} + +ParticleFilter::~ParticleFilter() { + int i; + for (i = 0; i < 2; i++) { + this->sample_set_ptr_array_[i]->clusters_vec.clear(); + this->sample_set_ptr_array_[i]->kd_tree_ptr.reset(); + this->sample_set_ptr_array_[i]->clusters_vec.shrink_to_fit(); + this->sample_set_ptr_array_[i]->samples_vec.clear(); + this->sample_set_ptr_array_[i]->samples_vec.shrink_to_fit(); + } + LOG_INFO << "Delete pf"; +} + +void ParticleFilter::InitByGuassian(const Vec3d &mean, const Mat3d &cov) { + int i; + auto sample_set_ptr = sample_set_ptr_array_[current_set_]; + + sample_set_ptr->kd_tree_ptr->Clear(); + + sample_set_ptr->sample_count = max_samples_; + + auto pf_gaussian_pdf = ParticleFilterGaussianPdf(mean, cov); + + for (i = 0; i < sample_set_ptr->sample_count; i++) { + sample_set_ptr->samples_vec[i].weight = 1.0 / max_samples_; + sample_set_ptr->samples_vec[i].pose = pf_gaussian_pdf.GenerateSample(); + sample_set_ptr->kd_tree_ptr->InsertPose(sample_set_ptr->samples_vec[i].pose, + sample_set_ptr->samples_vec[i].weight); + } + + this->w_slow_ = this->w_fast_ = 0.0; + + ClusterStatistics(sample_set_ptr); + InitConverged(); + +} + +bool ParticleFilter::UpdateConverged() { + auto sample_set_ptr = sample_set_ptr_array_[current_set_]; + double mean_x = 0, mean_y = 0; + + int i; + + for (i = 0; i < sample_set_ptr->sample_count; i++) { + mean_x += sample_set_ptr->samples_vec[i].pose(0); + mean_y += sample_set_ptr->samples_vec[i].pose(1); + } + mean_x /= sample_set_ptr->sample_count; + mean_y /= sample_set_ptr->sample_count; + + for (i = 0; i < sample_set_ptr->sample_count; i++) { + if (std::fabs(sample_set_ptr->samples_vec[i].pose(0) - mean_x) > this->dist_threshold_ || + std::fabs(sample_set_ptr->samples_vec[i].pose(1) - mean_y) > this->dist_threshold_) { + sample_set_ptr->converged = false; + this->converged_ = false; + return false; + } + } + + sample_set_ptr->converged = true; + this->converged_ = true; + return true; +} + +void ParticleFilter::InitConverged() { + this->sample_set_ptr_array_[current_set_]->converged = false; + this->converged_ = false; +} + +void ParticleFilter::ClusterStatistics() { + ClusterStatistics(this->sample_set_ptr_array_[0]); +} + +void ParticleFilter::ClusterStatistics(const SampleSetPtr &sample_set_ptr) { + int i, j, k; + int cluster_index; + ParticleFilterCluster *cluster; + + //work space + Mat2d tmp_ws_mat; + Vec4d tmp_ws_vec; + size_t count; + double weight; + + // Cluster the samples + sample_set_ptr->kd_tree_ptr->Cluster(); + + // Initialize cluster stats + sample_set_ptr->cluster_count = 0; + + for (i = 0; i < sample_set_ptr->cluster_max_count; i++) { + sample_set_ptr->clusters_vec[i].Reset(); + } + // Initialize overall filter stats + count = 0; + weight = 0.0; + sample_set_ptr->mean.setZero(); + sample_set_ptr->covariant.setZero(); + tmp_ws_mat.setZero(); + tmp_ws_vec.setZero(); + + // Compute cluster stats + for (i = 0; i < sample_set_ptr->sample_count; i++) { + auto sample_it = sample_set_ptr->samples_vec[i]; + // Get the cluster label for this sample + cluster_index = sample_set_ptr->kd_tree_ptr->GetCluster(sample_it.pose); + if (cluster_index < 0) { + LOG_ERROR << "Cluster not found"; + return; + } + if (cluster_index >= sample_set_ptr->cluster_max_count) { + continue; + } + if (cluster_index + 1 > sample_set_ptr->cluster_count) { + sample_set_ptr->cluster_count = cluster_index + 1; + } + + sample_set_ptr->clusters_vec[cluster_index].count += 1; + sample_set_ptr->clusters_vec[cluster_index].weight += sample_it.weight; + + count += 1; + weight += sample_it.weight; + + //compute mean + sample_set_ptr->clusters_vec[cluster_index].ws_vec(0) += sample_it.weight * sample_it.pose(0); + sample_set_ptr->clusters_vec[cluster_index].ws_vec(1) += sample_it.weight * sample_it.pose(1); + sample_set_ptr->clusters_vec[cluster_index].ws_vec(2) += sample_it.weight * cos(sample_it.pose(2)); + sample_set_ptr->clusters_vec[cluster_index].ws_vec(3) += sample_it.weight * sin(sample_it.pose(2)); + + tmp_ws_vec(0) += sample_it.weight * sample_set_ptr->clusters_vec[cluster_index].ws_vec(0); + tmp_ws_vec(1) += sample_it.weight * sample_set_ptr->clusters_vec[cluster_index].ws_vec(1); + tmp_ws_vec(2) += sample_it.weight * std::cos(sample_it.pose(2)); + tmp_ws_vec(3) += sample_it.weight * std::sin(sample_it.pose(2)); + + // Compute covariance in linear components + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + cluster = &sample_set_ptr->clusters_vec[cluster_index]; + cluster->cov(j, k) = cluster->ws_mat(j, k) / cluster->weight - + cluster->mean(j) * cluster->mean(k); + } + } + } + + // Normalize + for (i = 0; i < sample_set_ptr->cluster_count; i++) { + + cluster = &sample_set_ptr->clusters_vec[i]; + + cluster->mean(0) = cluster->ws_vec(0) / cluster->weight; + cluster->mean(1) = cluster->ws_vec(1) / cluster->weight; + cluster->mean(2) = std::atan2(cluster->ws_vec(3), cluster->ws_vec(2)); + + cluster->cov.setZero(); + + // Covariance in linear components + for (j = 0; j < 2; j++) { + for (k = 0; k < 2; k++) { + cluster->cov(j, k) = cluster->ws_mat(j, k) / cluster->weight - + cluster->mean(j) * cluster->mean(k); + } + } + + // Covariance in angular components + cluster->cov(2, 2) = -2 * std::log( + std::sqrt( + cluster->ws_vec(2) * cluster->ws_vec(2) + + cluster->ws_vec(3) * cluster->ws_vec(3) + )); + DLOG_INFO << "cluster: " << cluster->count + << "," << cluster->weight + << "," << cluster->mean(0) + << "," << cluster->mean(1) << "," << cluster->mean(2); + } + + // Compute overall filter stats + sample_set_ptr->mean(0) = tmp_ws_vec(0) / weight; + sample_set_ptr->mean(1) = tmp_ws_vec(1) / weight; + sample_set_ptr->mean(2) = std::atan2(tmp_ws_vec(3), tmp_ws_vec(2)); + + // Covariance in linear components + for (j = 0; j < 2; j++) { + for (k = 0; k < 2; k++) { + sample_set_ptr->covariant(j, k) = tmp_ws_mat(j, k) / weight - sample_set_ptr->mean(j) * sample_set_ptr->mean(k); + } + } + + // Covariance in angular components; + sample_set_ptr->covariant(2, 2) = + -2 * std::log(std::sqrt(tmp_ws_vec(2) * tmp_ws_vec(2) + tmp_ws_vec(3) * tmp_ws_vec(3))); + +} + +int ParticleFilter::GetClusterStatistics(int clabel, + double *weight, + Vec3d *mean, + Mat3d *cov) { + auto set = this->sample_set_ptr_array_[current_set_]; + if (clabel >= set->sample_count) { + return 0; + } + *weight = set->clusters_vec[clabel].weight; + *mean = set->clusters_vec[clabel].mean; + *cov = set->clusters_vec[clabel].cov; + + return 1; + +} + +SampleSetPtr ParticleFilter::GetCurrentSampleSetPtr() const { + return this->sample_set_ptr_array_[current_set_]; +} + +int ParticleFilter::ResampleLimit(int k) { + double a, b, c, x; + int n; + + if (k <= 1) { + LOG_WARNING_IF(k != 1) << "K < 1 (kd_tree leaf count < 1)"; + return this->max_samples_; + } + + a = 1.0; + b = 2.0 / (9.0 * ((double) k - 1.0)); + c = std::sqrt(2.0 / (9.0 * ((double) k - 1))) * this->pop_z_; + x = a - b + c; + + n = (int) std::ceil((k - 1.0) / (2.0 * this->pop_err_) * x * x * x); + +// LOG_INFO << "Required number of samples = "<< n; + + if (n < this->min_samples_) { + return this->min_samples_; + } + if (n > this->max_samples_) { + return this->max_samples_; + } + + return n; +} + +void ParticleFilter::UpdateOmega(double total_weight) { + int i = 0; + auto set = GetCurrentSampleSetPtr(); + + if (total_weight > 0.0) { + // Normalize weights + double w_avg = 0.0; + + for (i = 0; i < set->sample_count; i++) { + w_avg += set->samples_vec[i].weight; + set->samples_vec[i].weight /= total_weight; + } + + DLOG_INFO << "Update running averages of likelihood of samples"; //(Probabilistic Robotics p258) + + w_avg /= set->sample_count; + if (this->w_slow_ == 0.0) + this->w_slow_ = w_avg; + else + this->w_slow_ += this->alpha_slow_ * (w_avg - this->w_slow_); + if (this->w_fast_ == 0.0) + this->w_fast_ = w_avg; + else + this->w_fast_ += this->alpha_fast_ * (w_avg - this->w_fast_); + } else { + for (i = 0; i < set->sample_count; i++) { + auto sample = &set->samples_vec[i]; + sample->weight = 1.0 / set->sample_count; + } + } +} + +void ParticleFilter::UpdateResample() { + + double w_diff = 0; + double total = 0; + int i = 0; + + ParticleFilterSample *sample_a, *sample_b; + + auto set_a = this->sample_set_ptr_array_[current_set_]; + auto set_b = this->sample_set_ptr_array_[(current_set_ + 1) % 2]; + + // Build up cumulative probability table for resampling. + // TODO: Replace this with a more efficient procedure + // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html) + const int c_size = set_a->sample_count + 1; + std::vector c; + c.resize(c_size); + c[0] = 0.0; + for (i = 0; i < set_a->sample_count; i++) { + c[i + 1] = c[i] + set_a->samples_vec[i].weight; + } + + set_b->kd_tree_ptr->Clear(); + + // Draw samples from set a to create set b. + total = 0; + set_b->sample_count = 0; + + w_diff = 1.0 - this->w_fast_ / this->w_slow_; + if (w_diff < 0.0) { + w_diff = 0.0; + } + + while (set_b->sample_count < this->max_samples_) { + sample_b = &set_b->samples_vec[set_b->sample_count++]; + + if (drand48() < w_diff) + sample_b->pose = (random_pose_func_)(); + else { + double r; + r = drand48(); + + for (i = 0; i < set_a->sample_count; i++) { + if ((c[i] <= r) && (r < c[i + 1])) + break; + } + assert(i < set_a->sample_count); + sample_a = &(set_a->samples_vec[i]); + + assert(sample_a->weight > 0); + sample_b->pose = sample_a->pose; + } + + sample_b->weight = 1.0; + total += sample_b->weight; + + // Add sample to histogram + set_b->kd_tree_ptr->InsertPose(sample_b->pose, sample_b->weight); + + // See if we have enough samples yet + DLOG_INFO << "Histogram bins num: " << set_b->kd_tree_ptr->GetLeafCount(); + auto kld_resample_num = ResampleLimit(set_b->kd_tree_ptr->GetLeafCount()); + if (set_b->sample_count > kld_resample_num) { + LOG_INFO << "KLD-Resample num : " << kld_resample_num; + break; + } + } + + // Reset averages, to avoid spiraling off into complete randomness. + if (w_diff > 0.0) + this->w_slow_ = this->w_fast_ = 0.0; + + // Normalize weights + for (i = 0; i < set_b->sample_count; i++) { + sample_b = &(set_b->samples_vec[i]); + sample_b->weight /= total; + } + + // Re-compute cluster statistics + ClusterStatistics(set_b); + current_set_ = (current_set_ + 1) % 2; + UpdateConverged(); +// c.reset(); + +} + +void ParticleFilter::InitByModel(PfInitModelFunc init_fn) { + int i; + auto set = this->sample_set_ptr_array_[current_set_]; + // Create the kd tree for adaptive sampling + set->kd_tree_ptr->Clear(); + set->sample_count = max_samples_; + + // Compute the new sample poses + for (i = 0; i < set->sample_count; i++) { + set->samples_vec[i].weight = 1.0 / this->max_samples_; + + set->samples_vec[i].pose = init_fn();; + // Add sample to histogram + set->kd_tree_ptr->InsertPose(set->samples_vec[i].pose, set->samples_vec[i].weight); + } + + this->w_slow_ = this->w_fast_ = 0.0; + ClusterStatistics(set); + // Re-compute cluster statistics + InitConverged(); + //set converged to 0 +} + +void ParticleFilter::InitByGuassianWithRandomHeading(const Vec3d &mean, const Mat3d &cov) { + int i; + auto sample_set_ptr = sample_set_ptr_array_[current_set_]; + + LOG_INFO << "Init with random heading!"; + + sample_set_ptr->kd_tree_ptr->Clear(); + + sample_set_ptr->sample_count = max_samples_; + + auto pf_gaussian_pdf = ParticleFilterGaussianPdf(mean, cov); + + for (i = 0; i < sample_set_ptr->sample_count; i++) { + sample_set_ptr->samples_vec[i].weight = 1.0 / max_samples_; + sample_set_ptr->samples_vec[i].pose = pf_gaussian_pdf.GenerateSample(); + sample_set_ptr->samples_vec[i].pose[2] = drand48() * 2 * M_PI - M_PI; + sample_set_ptr->kd_tree_ptr->InsertPose(sample_set_ptr->samples_vec[i].pose, + sample_set_ptr->samples_vec[i].weight); + } + + this->w_slow_ = this->w_fast_ = 0.0; + + ClusterStatistics(sample_set_ptr); + InitConverged(); + +} + +void ParticleFilter::SetKldParam(double pop_err, double pop_z) { + this->pop_err_ = pop_err; + this->pop_z_ = pop_z; +} + +}// roborts_localization \ No newline at end of file diff --git a/roborts_localization/amcl/particle_filter/particle_filter.h b/roborts_localization/amcl/particle_filter/particle_filter.h new file mode 100755 index 00000000..180c4f05 --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License aranges_mat.setZero();long with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + + +#ifndef ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_H +#define ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_H + +#include +#include +#include + +#include "log.h" +#include "localization_math.h" +#include "map/amcl_map.h" +#include "particle_filter_sample.h" + +namespace roborts_localization { + +class ParticleFilter; + +using ParticleFilterPtr = std::shared_ptr; +using SampleSetPtr = std::shared_ptr; + +using PfInitModelFunc = std::function; + +/** + * @brief Particle filter class + */ +class ParticleFilter { + friend class SensorLaser; + friend class SensorOdom; + public: + /** + * @brief Constructor of particle filter class + * @param min_samples Min number of samples + * @param max_samples Max number of samples + * @param alpha_slow Decay rates for running averages + * @param alpha_fast Decay rates for running averages + * @param random_pose_func Function used to draw random pose samples + * @param random_pose_data + */ + ParticleFilter(int min_samples, + int max_samples, + double alpha_slow, + double alpha_fast, + const PfInitModelFunc &random_pose_func, + const std::shared_ptr &map_ptr + ); + + /** + * @brief Destructor + */ + virtual ~ParticleFilter(); + + /** + * @brief Initialize the filter using a guassian pdf + * @param mean Initial pose mean + * @param cov Initial pose covariant + */ + void InitByGuassian(const Vec3d &mean, const Mat3d &cov); + + /** + * @brief Initialize the filter using some model + * @param init_fn Model function + * @param init_data Model data + */ + void InitByModel(PfInitModelFunc init_fn); + + void InitByGuassianWithRandomHeading(const Vec3d &mean, const Mat3d &cov); + + void UpdateOmega(double total_weight); + + /** + * @brief Resample the distribution + */ + void UpdateResample(); + + /** + * @brief Compute the required number of samples, + * given that there are k bins with samples in them. + * This is taken directly from Fox et al. + * @param k Bins with samples + * @return Returns the required number of samples for resampling. + */ + int ResampleLimit(int k); + + /** + * @brief Re-compute the cluster statistics for a sample set + */ + void ClusterStatistics(); + + /** + * @brief Compute the statistics for a particular cluster. + * @param clabel Cluster label + * @param weight Weight of the cluster + * @param mean Mean of the cluster + * @param cov Cov of the cluster + * @return Returns 0 if there is no such cluster. + */ + int GetClusterStatistics(int clabel, double *weight, + Vec3d *mean, Mat3d *cov); + + void SetKldParam(double pop_err, double pop_z); + + SampleSetPtr GetCurrentSampleSetPtr() const; + + private: + void ClusterStatistics(const SampleSetPtr &sample_set_ptr); + void InitConverged(); + bool UpdateConverged(); + private: + + //! This min and max number of samples + int min_samples_ = 0, max_samples_ = 0; + + //! Population size parameters + double pop_err_, pop_z_; + + //! The sample sets. We keep two sets and use [current_set] to identify the active set. + int current_set_; + + std::array sample_set_ptr_array_; + + //! Running averages, slow and fast, of likelihood + double w_slow_, w_fast_; + + //! Decay rates for running averages + double alpha_slow_, alpha_fast_; + + //! Function used to draw random pose samples + PfInitModelFunc random_pose_func_; + std::shared_ptr map_ptr_; + + //! Distance threshold in each axis over which the pf is considered to not be converged + double dist_threshold_ = 0.5; + //! Particle filter converged flag + bool converged_ = false; +}; + +}// roborts_localization + + +#endif //ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_H diff --git a/roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.cpp b/roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.cpp new file mode 100755 index 00000000..2089fe8b --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distrmathibuted 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include "particle_filter_gaussian_pdf.h" + +namespace roborts_localization { + +ParticleFilterGaussianPdf::ParticleFilterGaussianPdf(const Vec3d &mean, + const Mat3d &covariance) { + this->mean_ = mean; + this->covariance_ = covariance; + math::EigenDecomposition(this->covariance_, this->covariance_rotation_, this->covariance_diagonal_); +} + +Vec3d ParticleFilterGaussianPdf::GenerateSample() { + int i = 0; + Vec3d random_vec; + Vec3d mean_vec; + + for (i = 0; i < this->covariance_diagonal_.size(); i++) { + double sigma = this->covariance_diagonal_(i); + random_vec(i) = math::RandomGaussianNumByStdDev(sigma); + } + + for (i = 0; i < this->mean_.size(); i++) { + mean_vec(i) = this->mean_(i); + for (int j = 0; j < this->mean_.size(); ++j) { + mean_vec(i) += this->covariance_rotation_(i, j) * random_vec(j); + } + } + + return mean_vec; +} + +}// roborts_localization + + diff --git a/roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.h b/roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.h new file mode 100755 index 00000000..cd14dcd0 --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_GAUSSIAN_PDF_H +#define ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_GAUSSIAN_PDF_H + +#include + +#include "localization_math.h" +#include "log.h" + +namespace roborts_localization { + +/** + * @brief Gaussian pdf class + */ +class ParticleFilterGaussianPdf { + + public: + /** + * @brief Default constructor + */ + ParticleFilterGaussianPdf() = default; + /** + * @brief Create a gaussian pdf by mean and covariance + * @param mean Mean to initialize the gaussian pdf + * @param covariance Covariance initialize the gaussian pdf + */ + ParticleFilterGaussianPdf(const Vec3d &mean, const Mat3d &covariance); + + /** + * @brief Generate random pose particle sample + * @return Return the random pose particle sample + */ + Vec3d GenerateSample(); + + private: + + Vec3d mean_; + Mat3d covariance_; + + // Decomposed covariance matrix (rotation * diagonal) + Mat3d covariance_rotation_; + Vec3d covariance_diagonal_; + +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_GAUSSIAN_PDF_H diff --git a/roborts_localization/amcl/particle_filter/particle_filter_kdtree.cpp b/roborts_localization/amcl/particle_filter/particle_filter_kdtree.cpp new file mode 100755 index 00000000..057879da --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter_kdtree.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include +#include "particle_filter_kdtree.h" + +namespace roborts_localization{ + +ParticleFilterKDTree::~ParticleFilterKDTree() { + delete (root_ptr_); + while (nodes_ptr_vec_.empty()) { + delete (nodes_ptr_vec_.back()); + nodes_ptr_vec_.pop_back(); + } + nodes_ptr_vec_.clear(); + nodes_ptr_vec_.shrink_to_fit(); + DLOG_INFO << "Destroy kd tree"; +} + +void ParticleFilterKDTree::InitializeByMaxSize(int max_size) { + + size_[0] = 0.5; + size_[1] = 0.5; + size_[2] = (10.0 * M_PI / 180.0); + node_count_ = 0; + node_max_count_ = max_size; + nodes_ptr_vec_.resize(node_max_count_); + leaf_count_ = 0; +} + +const int &ParticleFilterKDTree::GetLeafCount() const { + return leaf_count_; +} + +void ParticleFilterKDTree::Clear() { + root_ptr_ = nullptr; + while (nodes_ptr_vec_.empty()) { + delete (nodes_ptr_vec_.back()); + nodes_ptr_vec_.pop_back(); + } + nodes_ptr_vec_.clear(); + nodes_ptr_vec_.shrink_to_fit(); + leaf_count_ = 0; + node_count_ = 0; +} + +void ParticleFilterKDTree::InsertPose(Vec3d pose, double value) { + Vec3d key; + for (int i = 0; i < 3; i++) { + key[i] = std::floor(pose[i] / size_[i]); + } + root_ptr_ = InsertNode(nullptr, root_ptr_, key, value); +} + +ParticleFilterKDTreeNode *ParticleFilterKDTree::InsertNode( + ParticleFilterKDTreeNode *parent_node_ptr, + ParticleFilterKDTreeNode *node_ptr, + const Vec3d &key, + double value) { + + int i = 0; + int split = 0, max_split = 0; + if (node_ptr == nullptr) { + assert(node_count_ < node_max_count_); + node_count_++; + node_ptr = (new ParticleFilterKDTreeNode); + nodes_ptr_vec_.push_back(node_ptr); + nodes_ptr_vec_.back()->leaf = true; + if (parent_node_ptr == nullptr) { + nodes_ptr_vec_.back()->depth = 0; + } else { + nodes_ptr_vec_.back()->depth = parent_node_ptr->depth + 1; + } + nodes_ptr_vec_.back()->key = key; + nodes_ptr_vec_.back()->value = value; + this->leaf_count_ += 1; + //return nodes_ptr_vec_.back(); + + } else if (node_ptr->leaf) { + if (IsEqualKey(key, node_ptr->key)) { + node_ptr->value += value; + } else { + max_split = 0; + node_ptr->pivot_dim = -1; + for (i = 0; i < 3; i++) { + split = static_cast(std::fabs(key[i] - node_ptr->key[i])); + if (split > max_split) { + max_split = split; + node_ptr->pivot_dim = i; + } + } + assert(node_ptr->pivot_dim >= 0); + + node_ptr->pivot_value = (key(node_ptr->pivot_dim) + node_ptr->key(node_ptr->pivot_dim)) / 2.0; + + if (key[node_ptr->pivot_dim] < node_ptr->pivot_value) { + node_ptr->left_ptr = InsertNode(node_ptr, nullptr, key, value); + node_ptr->right_ptr = InsertNode(node_ptr, nullptr, node_ptr->key, node_ptr->value); + } else { + node_ptr->left_ptr = InsertNode(node_ptr, nullptr, node_ptr->key, node_ptr->value); + node_ptr->right_ptr = InsertNode(node_ptr, nullptr, key, value); + } + + node_ptr->leaf = false; + this->leaf_count_ -= 1; + } + } else { + assert(node_ptr->left_ptr != nullptr); + assert(node_ptr->right_ptr != nullptr); + + if (key[node_ptr->pivot_dim] < node_ptr->pivot_value) + InsertNode(node_ptr, node_ptr->left_ptr, key, value); + else + InsertNode(node_ptr, node_ptr->right_ptr, key, value); + + } + return node_ptr; +} + +int ParticleFilterKDTree::GetCluster(Vec3d pose) { + Vec3d key; + ParticleFilterKDTreeNode *node_ptr; + + for (int i = 0; i < pose.size(); i++) { + key(i) = std::floor(pose(i) / size_[i]); + } + + node_ptr = FindNode(root_ptr_, key); + if (node_ptr == nullptr) { + LOG_WARNING << "Cluster not found"; + return -1; + } + return node_ptr->cluster; + +} + +ParticleFilterKDTreeNode *ParticleFilterKDTree::FindNode(ParticleFilterKDTreeNode *node_ptr, Vec3d key) { + if (node_ptr->leaf) { + if (IsEqualKey(key, node_ptr->key)) { + return node_ptr; + } else { + return nullptr; + } + } else { + assert(node_ptr->left_ptr != nullptr); + assert(node_ptr->right_ptr != nullptr); + // If the keys are different... + if (key[node_ptr->pivot_dim] < node_ptr->pivot_value) + return FindNode(node_ptr->left_ptr, key); + else + return FindNode(node_ptr->right_ptr, key); + } + +} + +inline bool ParticleFilterKDTree::IsEqualKey(const Vec3d &key_a, const Vec3d &key_b) { + return key_a == key_b; +} + +void ParticleFilterKDTree::Cluster() { + + int i = 0; + int queue_count = 0, cluster_count = 0; + ParticleFilterKDTreeNode *node_ptr; + + std::unique_ptr queue(new ParticleFilterKDTreeNode *[this->node_count_]); + + for (i = 0; i < this->node_count_; i++) { + node_ptr = this->nodes_ptr_vec_[i]; + if (node_ptr->leaf) { + node_ptr->cluster = -1; + assert(queue_count < this->node_count_); + queue[queue_count++] = node_ptr; + } + } + + cluster_count = 0; + + while (queue_count > 0) { + node_ptr = queue[--queue_count]; + + if (node_ptr->cluster >= 0) { + continue; + } + + node_ptr->cluster = cluster_count++; + ClusterNode(node_ptr, 0); + } + queue.reset(); +// delete (queue); + DLOG_INFO << "Cluster count = " << cluster_count; +} + +void ParticleFilterKDTree::ClusterNode(ParticleFilterKDTreeNode *node_ptr, int depth) { + int i; + Vec3d nkey; + ParticleFilterKDTreeNode *n_node_ptr; + + for (i = 0; i < 3 * 3 * 3; i++) { + nkey(0) = node_ptr->key(0) + (i / 9) - 1; + nkey(1) = node_ptr->key(1) + ((i % 9) / 3) - 1; + nkey(2) = node_ptr->key(2) + ((i % 9) % 3) - 1; + + n_node_ptr = FindNode(root_ptr_, nkey); + if (n_node_ptr == nullptr) { + continue; + } + + assert(n_node_ptr->leaf); + + // This node already has a label; skip it. The label should be + // consistent, however. + if (n_node_ptr->cluster >= 0) { + continue; + } + + n_node_ptr->cluster = node_ptr->cluster; + ClusterNode(n_node_ptr, depth + 1); + } + return; +} + +}// roborts_localization \ No newline at end of file diff --git a/roborts_localization/amcl/particle_filter/particle_filter_kdtree.h b/roborts_localization/amcl/particle_filter/particle_filter_kdtree.h new file mode 100755 index 00000000..34e852e0 --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter_kdtree.h @@ -0,0 +1,139 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PATICLE_FILTER_KDTREE_H +#define ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PATICLE_FILTER_KDTREE_H + +#include +#include + +#include "log.h" +#include "localization_math.h" + +namespace roborts_localization{ + +/** + * @brief Information for a particle filter K-D tree node + */ +class ParticleFilterKDTreeNode { + public: + //! Left child node + ParticleFilterKDTreeNode *left_ptr; + //! Right child node + ParticleFilterKDTreeNode *right_ptr; + //! Leaf node flag + bool leaf = false; + //! Depth in the tree + int depth = 0; + //! The cluster label + int cluster = -1; + //! Pivot dimension + int pivot_dim = 0; + //! Pivot value + double pivot_value = 0; + //! The key for this node + Vec3d key = {0, 0, 0}; + //! The value for this node + double value = 0; +}; + +/** + * @brief K-D tree class + */ +class ParticleFilterKDTree { + public: + /** + * @brief Create a K-D tree by max sample size + * @param max_size Max sample size + */ + void InitializeByMaxSize(int max_size); + /** + * @brief Clear all entries from the tree + */ + void Clear(); + /** + * @brief Insert a pose into the tree + * @param pose Pose to insert + * @param value Weight of pose + */ + void InsertPose(Vec3d pose, double value); + /** + * @brief Cluster the leaves in the tree + */ + void Cluster(); + /** + * @brief Determine the cluster label for the given pose + * @param pose Pose + * @return Returns the cluster label of given pose if exist. + */ + int GetCluster(Vec3d pose); + /** + * @brief Get number of leaves + * @return Returns number of leaves. + */ + const int &GetLeafCount() const; + /** + * @brief Destuctor + */ + virtual ~ParticleFilterKDTree(); + private: + ParticleFilterKDTreeNode *InsertNode(ParticleFilterKDTreeNode *parent_node_ptr, + ParticleFilterKDTreeNode *node_ptr, + const Vec3d &key, + double value); + ParticleFilterKDTreeNode *FindNode(ParticleFilterKDTreeNode *node_ptr, Vec3d key); + inline bool IsEqualKey(const Vec3d &key_a, const Vec3d &key_b); + void ClusterNode(ParticleFilterKDTreeNode *node_ptr, int depth); + private: + //! Cell size + double size_[3] = {0.5,0.5,(10.0 * M_PI / 180.0)}; + //! The root node of the tree + ParticleFilterKDTreeNode *root_ptr_ = nullptr; + //! The number of nodes in the tree + int node_count_ = 0; + //! Max number of nodes in the tree + int node_max_count_ = 0; + //! Vector of nodes + std::vector nodes_ptr_vec_; + //! The number of leaf nodes in the tree + int leaf_count_ = 0; +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PATICLE_FILTER_KDTREE_H diff --git a/roborts_localization/amcl/particle_filter/particle_filter_sample.h b/roborts_localization/amcl/particle_filter/particle_filter_sample.h new file mode 100755 index 00000000..56570041 --- /dev/null +++ b/roborts_localization/amcl/particle_filter/particle_filter_sample.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_SAMPLE_H +#define ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_SAMPLE_H + +#include "localization_math.h" +#include "particle_filter_kdtree.h" + +namespace roborts_localization { + +/** + * @brief Information for a single sample. + */ +class ParticleFilterSample { + public: + ParticleFilterSample() { + Reset(); + } + /** + * @brief Particle filter sample initialize + */ + void Reset() { + pose.setZero(); + weight = 0; + }; + public: + //! Pose represented by this sample + Vec3d pose; + + //! Weight for this pose + double weight; +}; + +/** + * @brief Information for a cluster of samples + */ +class ParticleFilterCluster { + public: + ParticleFilterCluster() { + Reset(); + } + void Reset() { + count = 0; + weight = 0; + mean.setZero(); + cov.setZero(); + ws_vec.setZero(); + ws_mat.setZero(); + } + public: + //! Number of samples + int count; + //! Total weight of samples in this cluster + double weight; + //! Cluster statistics mean + Vec3d mean; + //! Cluster statistics cov + Mat3d cov; + //! Workspace 4 double vector + Vec4d ws_vec; + //! Workspace 2x2 double matrix + Mat2d ws_mat; +}; + +/** + * @brief Information for a set of samples + */ +class ParticleFilterSampleSet { + public: + //! Number of samples + int sample_count = 0; + //! Vector of samples + std::vector samples_vec; + //! Number of Clusters + int cluster_count = 0; + //! Max number of clusters + int cluster_max_count = 0; + //! Vector of clusters + std::vector clusters_vec; + //! A kdtree encoding the histogram + std::unique_ptr kd_tree_ptr; + //! Filter statistics mean + Vec3d mean; + //! Filter statistics covariant + Mat3d covariant; + //! Filter statistics converged status + bool converged = false; +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_SAMPLE_H diff --git a/roborts_localization/amcl/sensors/sensor_laser.cpp b/roborts_localization/amcl/sensors/sensor_laser.cpp new file mode 100755 index 00000000..5bf5b5e8 --- /dev/null +++ b/roborts_localization/amcl/sensors/sensor_laser.cpp @@ -0,0 +1,282 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include "sensor_laser.h" + +namespace roborts_localization { + +SensorLaser::SensorLaser(size_t max_beams, + const std::shared_ptr &map_ptr) : + max_samples_(0), + max_obs_(0) { + this->temp_obs_.clear(); + this->temp_obs_.shrink_to_fit(); + this->time_ = 0.0; + this->max_beams_ = max_beams; + this->map_ptr_ = map_ptr; +} + +void SensorLaser::SetModelLikelihoodFieldProb(double z_hit, + double z_rand, + double sigma_hit, + double max_occ_dist, + bool do_beamskip, + double beam_skip_distance, + double beam_skip_threshold, + double beam_skip_error_threshold, + double laser_filter_weight) { + + this->model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB; + this->z_hit_ = z_hit; + this->z_rand_ = z_rand; + this->sigma_hit_ = sigma_hit; + this->do_beamskip_ = do_beamskip; + this->beam_skip_distance_ = beam_skip_distance; + this->beam_skip_threshold_ = beam_skip_threshold; + this->beam_skip_error_threshold_ = beam_skip_error_threshold; + this->map_ptr_->UpdateCSpace(max_occ_dist); + laser_filter_weight_ = laser_filter_weight; +} + +double SensorLaser::LikelihoodFieldModelProb(SensorLaserData *sensor_laser_data_ptr, + SampleSetPtr sample_set_ptr) { + int i = 0, j = 0, step; + double z, pz; + double log_p; + double obs_range, obs_bearing; + double total_weight; + Vec3d pose; + Vec3d hit; + + total_weight = 0.0; + step = std::ceil((sensor_laser_data_ptr->range_count) / static_cast(this->max_beams_)); + + // Step size must be at least 1 + if (step < 1) { + step = 1; + } + + double z_hit_denom = 2 * this->sigma_hit_ * this->sigma_hit_; + double z_rand_mult = 1.0 / sensor_laser_data_ptr->range_max; + auto max_occ_dist = this->map_ptr_->GetMaxOccDist(); + double max_dist_prob = std::exp(-(max_occ_dist * max_occ_dist) / z_hit_denom); + + //Beam skipping - ignores beams for which a majoirty of particles do not agree with the map + //prevents correct particles from getting down weighted because of unexpected obstacles + //such as humans + bool do_beamskip = this->do_beamskip_; + double beam_skip_distance = this->beam_skip_distance_; + double beam_skip_threshold = this->beam_skip_threshold_; + + //we only do beam skipping if the filter has converged + if (do_beamskip && !sample_set_ptr->converged) { + do_beamskip = false; + DLOG_INFO << "Filter not converged"; + } + + //we need a count the no of particles for which the beam agreed with the map + //we also need a mask of which observations to integrate (to decide which beams to integrate to all particles) + std::unique_ptr obs_count(new int[this->max_beams_]); + std::unique_ptr obs_mask(new bool[this->max_beams_]); + + int beam_ind = 0; + + //reset indicates if we need to reallocate the temp data structure needed to do beamskipping + bool reset = false; + + if (do_beamskip) { + + if (this->max_obs_ < this->max_beams_) { + reset = true; + } + + if (this->max_samples_ < sample_set_ptr->sample_count) { + reset = true; + } + + if (reset) { + this->ResetTempData(sample_set_ptr->sample_count, this->max_beams_); + DLOG_INFO << "Reallocing temp weights " << this->max_samples_ << " - " << this->max_obs_; + } + } + + //Compute the sample weights + + for (j = 0; j < sample_set_ptr->sample_count; j++) { + + pose = sample_set_ptr->samples_vec.at(j).pose; + pose = math::CoordAdd(this->laser_pose_, pose); + log_p = 0; + beam_ind = 0; + + for (i = 0; i < sensor_laser_data_ptr->range_count; i += step, beam_ind++) { + obs_range = sensor_laser_data_ptr->ranges_mat(i, 0); + obs_bearing = sensor_laser_data_ptr->ranges_mat(i, 1); + + // This model ignores max range readings + if (obs_range >= sensor_laser_data_ptr->range_max) { + continue; + } + + // Check for NaN + if (obs_range != obs_range) { + continue; + } + + pz = 0.0; + + // Compute the endpoint of the beam + hit(0) = pose(0) + obs_range * std::cos(pose(2) + obs_bearing); + hit(1) = pose(1) + obs_range * std::sin(pose(2) + obs_bearing); + + // Convert to map grid coords. + int mi, mj; + this->map_ptr_->ConvertWorldCoordsToMapCoords(hit(0), hit(1), mi, mj); + + // Part 1: Get distance from the hit to closest obstacle. + // Off-map penalized as max distance + if (!this->map_ptr_->CheckMapCoordsValid(mi, mj)) { + pz += this->z_hit_ * max_dist_prob; + } else { + + int cell_ind = this->map_ptr_->ComputeCellIndexByMap(mi, mj); + z = this->map_ptr_->GetCellOccDistByIndex(cell_ind); + if (z < beam_skip_distance) { + obs_count[beam_ind] += 1; + } + pz += this->z_hit_ * std::exp(-(z * z) / z_hit_denom); + } + + // Gaussian model + // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) + + // Part 2: random measurements + pz += this->z_rand_ * z_rand_mult; + + LOG_FATAL_IF(pz > 1.0 || pz < 0.0) << "pz error num = " << pz; + + if (!do_beamskip) { + log_p += log(pz); + } else { + CHECK_GT(this->temp_obs_.size(), 0); + CHECK_GT(this->temp_obs_.at(j).size(), 0); + this->temp_obs_.at(j).at(beam_ind) = pz; + } + } + + if (!do_beamskip) { + sample_set_ptr->samples_vec.at(j).weight *= exp(log_p); + total_weight += sample_set_ptr->samples_vec.at(j).weight; + } + + } + + //Skip part of beams + if (do_beamskip) { + int skipped_beam_count = 0; + for (beam_ind = 0; beam_ind < this->max_beams_; beam_ind++) { + if ((obs_count[beam_ind] / static_cast(sample_set_ptr->sample_count)) > beam_skip_threshold) { + obs_mask[beam_ind] = true; + } else { + obs_mask[beam_ind] = false; + skipped_beam_count++; + } + } + DLOG(INFO) << "skipped_beam_count = " << skipped_beam_count << " max_beams = " << this->max_beams_; + //we check if there is at least a critical number of beams that agreed with the map + //otherwise it probably indicates that the filter converged to a wrong solution + //if that's the case we integrate all the beams and hope the filter might converge to + //the right solution + bool error = false; + if (skipped_beam_count >= (beam_ind * this->beam_skip_error_threshold_)) { + LOG_ERROR << "Over " << (100 * this->beam_skip_error_threshold_) + << " of the observations were not in the map - " + << "pf may have converged to wrong pose - " + << "integrating all observations"; + error = true; + } + + for (j = 0; j < sample_set_ptr->sample_count; j++) { + pose = sample_set_ptr->samples_vec.at(j).pose; + log_p = 0; + for (beam_ind = 0; beam_ind < this->max_beams_; beam_ind++) { + if (error || obs_mask[beam_ind]) { + LOG_FATAL_IF(j > this->temp_obs_.size() - 1) << "temp_obs size = " + << this->temp_obs_.size() + << "j=" + << j; + LOG_FATAL_IF(beam_ind > this->temp_obs_.at(j).size() - 1) << "temp_obs at j size = " + << this->temp_obs_.at(j).size() + << "beam_ind = " << beam_ind; + log_p += std::log(this->temp_obs_.at(j).at(beam_ind)); + } + } + sample_set_ptr->samples_vec.at(j).weight *= std::exp(log_p); + total_weight += sample_set_ptr->samples_vec.at(j).weight; + } + } + + return (total_weight); +}; + +double SensorLaser::UpdateSensor(ParticleFilterPtr pf_ptr, SensorLaserData *sensor_data_ptr) { + if (this->max_beams_ < 2) + return 0; + auto set = pf_ptr->GetCurrentSampleSetPtr(); + // Apply the laser sensor model + return LikelihoodFieldModelProb(sensor_data_ptr, set); +} + +void SensorLaser::ResetTempData(int new_max_samples, int new_max_obs) { + + max_obs_ = new_max_obs; + max_samples_ = std::max(max_samples_, new_max_samples); + DLOG_INFO << __FUNCTION__ << ": New max obs = " << max_obs_ << "New max samples = " << max_samples_; + CHECK_GT(max_samples_, 0); + temp_obs_.resize(max_samples_); + for (int k = 0; k < max_samples_; k++) { + CHECK_GT(max_obs_, 0); + temp_obs_[k].resize(max_obs_, 0); + } +} + +void SensorLaser::SetLaserPose(const Vec3d &laser_pose) { + laser_pose_ = laser_pose; +} + +}// roborts_localization \ No newline at end of file diff --git a/roborts_localization/amcl/sensors/sensor_laser.h b/roborts_localization/amcl/sensors/sensor_laser.h new file mode 100755 index 00000000..ed8240d1 --- /dev/null +++ b/roborts_localization/amcl/sensors/sensor_laser.h @@ -0,0 +1,197 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_AMCL_SENSORS_SENSOR_LASER_H +#define ROBORTS_LOCALIZATION_AMCL_SENSORS_SENSOR_LASER_H + +#include + +#include "particle_filter/particle_filter.h" +#include "localization_math.h" +#include "log.h" + +namespace roborts_localization { + +enum LaserModel { + LASER_MODEL_BEAM = 0, + LASER_MODEL_LIKELIHOOD_FIELD = 1, + LASER_MODEL_LIKELIHOOD_FIELD_PROB = 2 +}; + +/** + * @brief Laser data class + */ +class SensorLaserData { + public: + /** + * @brief Default constructor. + */ + SensorLaserData() { + range_count = 0; + range_max = 0; + ranges_mat.setZero(); + } + + /** + * @brief Default destructor. + */ + virtual ~SensorLaserData() = default; + public: + + /** + * @brief Ranges count of laser + */ + int range_count; + + /** + * @brief Max range of laser + */ + double range_max; + + /** + * @brief Laser ranges data + */ +// DynamicMat ranges_mat; + MatX2d ranges_mat; +// Eigen::MatrixX2d ranges_mat; +}; + +/** + * @brief Laser sensor model class + */ +class SensorLaser { + public: + /** + * @brief Constructor function + * @param max_beams Laser max beams number + * @param map_ptr AmclMap object pointer + */ + SensorLaser(size_t max_beams, const std::shared_ptr &map_ptr); + + /** + * @brief Initialize laser likelihood field model. + * @param z_hit Measurement noise coefficient + * @param z_rand Random measurement noise coefficient + * @param sigma_hit Stddev of Gaussian model for laser hits + * @param max_occ_dist Max distance at which we care about obstacles + * @param do_beamskip Beam skipping option + * @param beam_skip_distance Beam skipping distance + * @param beam_skip_threshold Beam skipping threshold + * @param beam_skip_error_threshold Threshold for the ratio of invalid beams - + * at which all beams are integrated to the likelihoods. + * This would be an error condition + */ + void SetModelLikelihoodFieldProb(double z_hit, + double z_rand, + double sigma_hit, + double max_occ_dist, + bool do_beamskip, + double beam_skip_distance, + double beam_skip_threshold, + double beam_skip_error_threshold, + double laser_filter_weight); + + /** + * @brief Update the filter based on the sensor model. + * @param pf_ptr Particle filter object pointer + * @param sensor_data_ptr Sensor data object pointer + * @return Returns total weight of samples. + */ + double UpdateSensor(ParticleFilterPtr pf_ptr, SensorLaserData *sensor_data_ptr); + + /** + * @brief Determine the probability for the given pose. A more probabilistically correct model + * - also with the option to do beam skipping + * @param sensor_laser_data_ptr Sensor data object pointer + * @param sample_set_ptr Paticle sample set object pointer + * @return Returns weight of given pose. + */ + double LikelihoodFieldModelProb(SensorLaserData *sensor_laser_data_ptr, + SampleSetPtr sample_set_ptr); + + /** + * @brief Set laser pose + * @param laser_pose Laser pose to set + */ + void SetLaserPose(const Vec3d &laser_pose); + + private: + + void ResetTempData(int new_max_samples, int new_max_obs); + + private: + + LaserModel model_type_; + + double time_; + + /** + * @brief AmclMap object pointer + */ + std::shared_ptr map_ptr_; + + Vec3d laser_pose_; + int max_beams_; + bool do_beamskip_; + double beam_skip_distance_; + double beam_skip_threshold_; + double beam_skip_error_threshold_; + + int max_samples_ = 0; + int max_obs_ = 0; + std::vector> temp_obs_; + + // Laser model params + // Mixture params for the components of the model; must sum to 1 + double z_hit_; + double z_short_; + double z_max_; + double z_rand_; + + double laser_filter_weight_; + + /** + * @brief Stddev of Gaussian model for laser hits. + */ + double sigma_hit_; + +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_SENSORS_SENSOR_LASER_H diff --git a/roborts_localization/amcl/sensors/sensor_odom.cpp b/roborts_localization/amcl/sensors/sensor_odom.cpp new file mode 100755 index 00000000..3a1be476 --- /dev/null +++ b/roborts_localization/amcl/sensors/sensor_odom.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include "log.h" +#include "sensor_odom.h" + +namespace roborts_localization{ + +SensorOdom::SensorOdom(double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5) : + alpha1_(alpha1), + alpha2_(alpha2), + alpha3_(alpha3), + alpha4_(alpha4), + alpha5_(alpha5), + time_(0.0){ + odom_model_type_ = ODOM_MODEL_OMNI; +} + +//TODO(kevin.li): This version only support omni model now. We will add diff model in the future. + +void SensorOdom::SetModelOmni(double alpha1, double alpha2, double alpha3, double alpha4, double alpha5) { + odom_model_type_ = ODOM_MODEL_OMNI; + alpha1_ = alpha1; + alpha2_ = alpha2; + alpha3_ = alpha3; + alpha4_ = alpha4; + alpha5_ = alpha5; +} + +bool SensorOdom::UpdateAction(SampleSetPtr sample_set_ptr, const SensorOdomData &odom_data) { + + DLOG_INFO << "Compute the new sample poses by motion model"; + + Vec3d old_pose = (odom_data.pose) - (odom_data.delta); + + double delta_bearing; + double delta_trans_hat, delta_rot_hat, delta_strafe_hat; + double delta_trans = std::sqrt(odom_data.delta[0] * odom_data.delta[0] + odom_data.delta[1] * odom_data.delta[1]); + double delta_rot = odom_data.delta[2]; + + double trans_hat_stddev = std::sqrt(alpha3_ * (delta_trans * delta_trans) + alpha1_ * (delta_rot * delta_rot)); + double rot_hat_stddev = std::sqrt(alpha4_ * (delta_rot * delta_rot) + alpha2_ * (delta_trans * delta_trans)); + double strafe_hat_stddev = std::sqrt(alpha1_ * (delta_rot * delta_rot) + alpha5_ * (delta_trans * delta_trans)); + + for (int i = 0; i < sample_set_ptr->sample_count; i++) { + + delta_bearing = math::AngleDiff(std::atan2(odom_data.delta(1), + odom_data.delta(0)), + old_pose(2)) + sample_set_ptr->samples_vec[i].pose(2); + + double cs_bearing = std::cos(delta_bearing); + double sn_bearing = std::sin(delta_bearing); + + delta_trans_hat = delta_trans + math::RandomGaussianNumByStdDev(trans_hat_stddev); + delta_rot_hat = delta_rot + math::RandomGaussianNumByStdDev(rot_hat_stddev); + delta_strafe_hat = 0 + math::RandomGaussianNumByStdDev(strafe_hat_stddev); + + sample_set_ptr->samples_vec[i].pose[0] += (delta_trans_hat * cs_bearing + delta_strafe_hat * sn_bearing); + sample_set_ptr->samples_vec[i].pose[1] += (delta_trans_hat * sn_bearing - delta_strafe_hat * cs_bearing); + sample_set_ptr->samples_vec[i].pose[2] += delta_rot_hat; + + } + return true; +} + +}// roborts_localization + diff --git a/roborts_localization/amcl/sensors/sensor_odom.h b/roborts_localization/amcl/sensors/sensor_odom.h new file mode 100755 index 00000000..3b1c23c7 --- /dev/null +++ b/roborts_localization/amcl/sensors/sensor_odom.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_AMCL_SENSORS_SENSOR_ODOM_H +#define ROBORTS_LOCALIZATION_AMCL_SENSORS_SENSOR_ODOM_H + +#include "../particle_filter/particle_filter.h" +#include "localization_math.h" + +namespace roborts_localization{ + +enum OdomModel{ + ODOM_MODEL_DIFF = 0, + ODOM_MODEL_OMNI = 1 +}; + + +class SensorOdomData{ + public: + Vec3d pose; + Vec3d delta; +}; + +class SensorOdom { + public: + /** + * @brief Default constructor + */ + SensorOdom(double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5); + + /** + * @brief Set odometry model to omni model. + * @param alpha1 + * @param alpha2 + * @param alpha3 + * @param alpha4 + * @param alpha5 + */ + void SetModelOmni(double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5); + + /** + * @brief Update the filter based on the action model + * @param pf_ptr Particle filter object pointer + * @param sensor_data_ptr Sensor data object pointer + * @return Returns true if the filter has been updated + */ + bool UpdateAction(SampleSetPtr pf_sample_set_ptr, + const SensorOdomData &odom_data); + + private: + + /** + * @brief Current data timestamp + */ + double time_; + + /** + * @brief Model type + */ + OdomModel odom_model_type_; + + /** + * @brief Drift parameters + */ + double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_AMCL_SENSORS_SENSOR_ODOM_H diff --git a/roborts_localization/cmake_module/FindEigen3.cmake b/roborts_localization/cmake_module/FindEigen3.cmake new file mode 100755 index 00000000..239c1bbe --- /dev/null +++ b/roborts_localization/cmake_module/FindEigen3.cmake @@ -0,0 +1,86 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/roborts_localization/cmake_module/FindGlog.cmake b/roborts_localization/cmake_module/FindGlog.cmake new file mode 100755 index 00000000..99abbe47 --- /dev/null +++ b/roborts_localization/cmake_module/FindGlog.cmake @@ -0,0 +1,48 @@ +# - Try to find Glog +# +# The following variables are optionally searched for defaults +# GLOG_ROOT_DIR: Base directory where all GLOG components are found +# +# The following are set after configuration is done: +# GLOG_FOUND +# GLOG_INCLUDE_DIRS +# GLOG_LIBRARIES +# GLOG_LIBRARYRARY_DIRS + +include(FindPackageHandleStandardArgs) + +set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") + +if(WIN32) + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}/src/windows) +else() + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}) +endif() + +if(MSVC) + find_library(GLOG_LIBRARY_RELEASE libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Release) + + find_library(GLOG_LIBRARY_DEBUG libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Debug) + + set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) +else() + find_library(GLOG_LIBRARY glog + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES lib lib64) +endif() + +find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) + +if(GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) + message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") + mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG + GLOG_LIBRARY GLOG_INCLUDE_DIR) +endif() diff --git a/roborts_localization/config/localization.yaml b/roborts_localization/config/localization.yaml new file mode 100644 index 00000000..00c4bf2e --- /dev/null +++ b/roborts_localization/config/localization.yaml @@ -0,0 +1,21 @@ +odom_frame_id : "odom" +base_frame_id : "base_link" +global_frame_id : "map" +laser_topic_name : "scan" +map_topic_name : "map" +init_pose_topic_name : "initialpose" +transform_tolerance : 0.1 +#8.40 #1.0 +#14.35 #1.0 +initial_pose_x : 1 +initial_pose_y : 1 +initial_pose_a : 3.14 +initial_cov_xx : 0.1 +initial_cov_yy : 0.1 +initial_cov_aa : 0.1 + +enable_uwb : false +uwb_frame_id : "uwb" +uwb_topic_name : "uwb" +use_sim_uwb : false +uwb_correction_frequency : 20 diff --git a/roborts_localization/localization_config.h b/roborts_localization/localization_config.h new file mode 100644 index 00000000..6eb56015 --- /dev/null +++ b/roborts_localization/localization_config.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include + +#ifndef ROBORTS_LOCALIZATION_LOCALIZATION_CONFIG_H +#define ROBORTS_LOCALIZATION_LOCALIZATION_CONFIG_H + +namespace roborts_localization { + +// Get Parameters from ROS parameter server +struct LocalizationConfig { + void GetParam(ros::NodeHandle *nh) { + nh->param("odom_frame", odom_frame_id, "odom"); + nh->param("base_frame", base_frame_id, "base_link"); + nh->param("global_frame", global_frame_id, "map"); + nh->param("laser_topic_name", laser_topic_name, "scan"); + nh->param("map_topic_name", map_topic_name, "map"); + nh->param("init_pose_topic_name", init_pose_topic_name, "initialpose"); + nh->param("transform_tolerance", transform_tolerance, 0.1); + nh->param("initial_pose_x", initial_pose_x, 1); + nh->param("initial_pose_y", initial_pose_y, 1); + nh->param("initial_pose_a", initial_pose_a, 0); + nh->param("initial_cov_xx", initial_cov_xx, 0.1); + nh->param("initial_cov_yy", initial_cov_yy, 0.1); + nh->param("initial_cov_aa", initial_cov_aa, 0.1); + nh->param("enable_uwb", enable_uwb, false); + nh->param("uwb_frame_id", uwb_frame_id, "uwb"); + nh->param("uwb_topic_name", uwb_topic_name, "uwb"); + nh->param("use_sim_uwb", use_sim_uwb, false); + nh->param("uwb_correction_frequency", uwb_correction_frequency, 20); + nh->param("publish_visualize", publish_visualize, true); + } + std::string odom_frame_id; + std::string base_frame_id; + std::string global_frame_id; + + std::string laser_topic_name; + std::string map_topic_name; + std::string init_pose_topic_name; + + double transform_tolerance; + + double initial_pose_x; + double initial_pose_y; + double initial_pose_a; + double initial_cov_xx; + double initial_cov_yy; + double initial_cov_aa; + + bool publish_visualize; + + bool enable_uwb; + std::string uwb_frame_id; + std::string uwb_topic_name; + bool use_sim_uwb; + int uwb_correction_frequency; + +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_LOCALIZATION_CONFIG_H diff --git a/roborts_localization/localization_math.cpp b/roborts_localization/localization_math.cpp new file mode 100644 index 00000000..2193d31a --- /dev/null +++ b/roborts_localization/localization_math.cpp @@ -0,0 +1,290 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "localization_math.h" + +namespace roborts_localization { +namespace math { + +Vec3d CoordAdd(const Vec3d &a, const Vec3d &b) { + Vec3d c; + c(0) = b(0) + a(0) * std::cos(b(2)) - a(1) * std::sin(b(2)); + c(1) = b(1) + a(0) * std::sin(b(2)) + a(1) * std::cos(b(2)); + c(2) = b(2) + a(2); + c(2) = std::atan2(sin(c(2)), cos(c(2))); + return c; +}; + +void EigenDecomposition(const Mat3d &matrix_a, + Mat3d &matrix_v, + Vec3d &vector_d) { + int i, j; + Vec3d vector_e; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + (matrix_v)(i, j) = matrix_a(i, j); + } + } + + Tred2(matrix_v, vector_d, vector_e); + Tql2(matrix_v, vector_d, vector_e); + +} + +inline void Tred2(Mat3d &matrix_v, Vec3d &vector_d, Vec3d &vector_e) { + +// This is derived from the Algol procedures tred2 by +// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for +// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding +// Fortran subroutine in EISPACK. + + int i, j, k; + double f, g, h, hh; + for (j = 0; j < 3; j++) { + vector_d(j) = matrix_v(2, j); + } + + // Householder reduction to tridiagonal form. + + for (i = 3 - 1; i > 0; i--) { + + // Scale to avoid under/overflow. + + double scale = 0.0; + double h = 0.0; + for (k = 0; k < i; k++) { + scale = scale + fabs(vector_d(k)); + } + if (scale == 0.0) { + vector_e[i] = vector_d(i - 1); + for (j = 0; j < i; j++) { + vector_d(j) = matrix_v(i - 1, j); + matrix_v(i, j) = 0.0; + matrix_v(j, i) = 0.0; + } + } else { + + // Generate Householder vector. + + for (k = 0; k < i; k++) { + vector_d(k) /= scale; + h += vector_d(k) * vector_d(k); + } + f = vector_d(i - 1); + g = sqrt(h); + if (f > 0) { + g = -g; + } + vector_e(i) = scale * g; + h = h - f * g; + vector_d(i - 1) = f - g; + for (j = 0; j < i; j++) { + vector_e(j) = 0.0; + } + + // Apply similarity transformation to remaining columns. + + for (j = 0; j < i; j++) { + f = vector_d(j); + matrix_v(j, i) = f; + g = vector_e(j) + matrix_v(j, j) * f; + for (k = j + 1; k <= i - 1; k++) { + g += matrix_v(k, j) * vector_d(k); + vector_e[k] += matrix_v(k, j) * f; + } + vector_e[j] = g; + } + f = 0.0; + for (j = 0; j < i; j++) { + vector_e(j) /= h; + f += vector_e(j) * vector_d(j); + } + hh = f / (h + h); + for (j = 0; j < i; j++) { + vector_e(j) -= hh * vector_d(j); + } + for (j = 0; j < i; j++) { + f = vector_d(j); + g = vector_e(j); + for (k = j; k <= i - 1; k++) { + matrix_v(k, j) -= (f * vector_e(k) + g * vector_d(k)); + } + vector_d[j] = matrix_v(i - 1, j); + matrix_v(i, j) = 0.0; + } + } + vector_d(i) = h; + } + + // Accumulate transformations. + + for (i = 0; i < 2; i++) { + matrix_v(2, i) = matrix_v(i, i); + matrix_v(i, i) = 1.0; + h = vector_d(i + 1); + if (h != 0.0) { + for (k = 0; k <= i; k++) { + vector_d(k) = matrix_v(k, i + 1) / h; + } + for (j = 0; j <= i; j++) { + g = 0.0; + for (k = 0; k <= i; k++) { + g += matrix_v(k, i + 1) * matrix_v(k, j); + } + for (k = 0; k <= i; k++) { + matrix_v(k, j) -= g * vector_d(k); + } + } + } + for (k = 0; k <= i; k++) { + matrix_v(k, i + 1) = 0.0; + } + } + for (j = 0; j < 3; j++) { + vector_d(j) = matrix_v(2, j); + matrix_v(2, j) = 0.0; + } + matrix_v(2, 2) = 1.0; + vector_e(0) = 0.0; +} + +inline void Tql2(Mat3d &matrix_v, Vec3d &vector_d, Vec3d &vector_e) { + +// This is derived from the Algol procedures tql2, by +// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for +// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding +// Fortran subroutine in EISPACK. + + int i, j, m, l, k; + double g, p, r, dl1, h, f, tst1, eps; + double c, c2, c3, el1, s, s2; + + for (i = 1; i < 3; i++) { + vector_e[i - 1] = vector_e[i]; + } + vector_e[3 - 1] = 0.0; + + f = 0.0; + tst1 = 0.0; + eps = pow(2.0, -52.0); + for (l = 0; l < 3; l++) { + + // Find small subdiagonal element + + tst1 = std::max(tst1, fabs(vector_d[l]) + fabs(vector_e[l])); + m = l; + while (m < 3) { + if (fabs(vector_e[m]) <= eps * tst1) { + break; + } + m++; + } + + // If m == l, d[l] is an eigenvalue, + // otherwise, iterate. + + if (m > l) { + int iter = 0; + do { + iter = iter + 1; // (Could check iteration count here.) + + // Compute implicit shift + + g = vector_d[l]; + p = (vector_d[l + 1] - g) / (2.0 * vector_e[l]); + r = std::sqrt(p * p + 1.0 * 1.0); +// r = hypot2(p,1.0); + if (p < 0) { + r = -r; + } + vector_d[l] = vector_e[l] / (p + r); + vector_d[l + 1] = vector_e[l] * (p + r); + dl1 = vector_d[l + 1]; + h = g - vector_d[l]; + for (i = l + 2; i < 3; i++) { + vector_d[i] -= h; + } + f = f + h; + + // Implicit QL transformation. + + p = vector_d[m]; + c = 1.0; + c2 = c; + c3 = c; + el1 = vector_e[l + 1]; + s = 0.0; + s2 = 0.0; + for (i = m - 1; i >= l; i--) { + c3 = c2; + c2 = c; + s2 = s; + g = c * vector_e[i]; + h = c * p; + r = std::sqrt(p * p + vector_e[i] * vector_e[i]); + vector_e[i + 1] = s * r; + s = vector_e[i] / r; + c = p / r; + p = c * vector_d[i] - s * g; + vector_d[i + 1] = h + s * (c * g + s * vector_d[i]); + + // Accumulate transformation. + + for (k = 0; k < 3; k++) { + h = matrix_v(k, i + 1); + matrix_v(k, i + 1) = s * matrix_v(k, i) + c * h; + matrix_v(k, i) = c * matrix_v(k, i) - s * h; + } + } + p = -s * s2 * c3 * el1 * vector_e[l] / dl1; + vector_e[l] = s * p; + vector_d[l] = c * p; + + // Check for convergence. + + } while (fabs(vector_e[l]) > eps * tst1); + } + vector_d[l] = vector_d[l] + f; + vector_e[l] = 0.0; + } + + // Sort eigenvalues and corresponding vectors. + + for (i = 0; i < 3 - 1; i++) { + k = i; + p = vector_d[i]; + for (j = i + 1; j < 3; j++) { + if (vector_d[j] < p) { + k = j; + p = vector_d[j]; + } + } + if (k != i) { + vector_d[k] = vector_d[i]; + vector_d[i] = p; + for (j = 0; j < 3; j++) { + p = matrix_v(j, i); + matrix_v(j, i) = matrix_v(j, k); + matrix_v(j, k) = p; + } + } + } +} + +}// math + +}// roborts_localization diff --git a/roborts_localization/localization_math.h b/roborts_localization/localization_math.h new file mode 100755 index 00000000..1a030434 --- /dev/null +++ b/roborts_localization/localization_math.h @@ -0,0 +1,177 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef ROBORTS_LOCALIZATION_R_MATH_H +#define ROBORTS_LOCALIZATION_R_MATH_H + +#include +#include +#include +#include "types.h" + +namespace roborts_localization { +namespace math { + +/** + * @brief Angle normalization. Radian angle => [-pi,pi] + * @param z Angle in radians + * @return Returns normalized radian angle in [-pi,pi] + */ +template +T Normalize(T z) { + return std::atan2(std::sin(z), std::cos(z)); +}; + +/** + * @brief Calculate difference between two angles. + * @param a Angle a + * @param b Angle b + * @return Returns angle difference in radians. + */ +template +inline T AngleDiff(T a, T b) { + a = Normalize(a); + b = Normalize(b); + double d1 = a - b; + double d2 = 2.0 * M_PI - std::fabs(d1); + if (d1 > 0) { + d2 *= -1.0; + } + if (std::fabs(d1) < std::fabs(d2)) { + return d1; + } else { + return d2; + } +}; + +/** + * @brief Add coordinates of two pose vectors. + * @param a Pose vector a (x, y , yaw) + * @param b Pose vector a (x, y , yaw) + * @return Returns the result pose. + */ +Vec3d CoordAdd(const Vec3d &a, const Vec3d &b); + +template +inline T RandomGaussianNum(T cov) { + T sigma = std::sqrt(std::abs(cov)); + T x1, x2, w, r; + do { + do { + r = drand48(); + } while (r == 0.0); + x1 = 2.0 * r - 1.0; + do { + r = drand48(); + } while (r == 0.0); + x2 = 2.0 * r - 1.0; + w = x1 * x1 + x2 * x2; + } while (w > 1.0 || w == 0.0); + return (sigma * x2 * std::sqrt(-2.0 * std::log(w) / w)); +} + +/** + * @brief Generate random number from a zero-mean Gaussian distribution, with standard deviation sigma + * @param sigma Standard deviation of gaussian distribution + * @return Random double number from the gaussian distribution + */ +template +inline T RandomGaussianNumByStdDev(T sigma) { + T x1, x2, w, r; + do { + do { + r = drand48(); + } while (r == 0.0); + x1 = 2.0 * r - 1.0; + do { + r = drand48(); + } while (r == 0.0); + x2 = 2.0 * r - 1.0; + w = x1 * x1 + x2 * x2; + } while (w > 1.0 || w == 0.0); + return (sigma * x2 * std::sqrt(-2.0 * std::log(w) / w)); +}; + +template +T EuclideanDistance(T x1, T y1, T x2, T y2) { + return std::sqrt(std::pow((x1 - x2), 2) + std::pow((y1 - y2), 2)); +} + +template +inline bool Near(T num1, T num2, T bound) { + if (num1 > num2 + bound) { + return false; + } else if (num1 + bound < num2) { + return false; + } else { + return true; + } +} + +inline Mat3d MsgCovarianceToMat3d(const boost::array &msg_cov) { + Mat3d pose_cov; + pose_cov.setZero(); + + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + pose_cov(i, j) = msg_cov[6 * i + j]; + } + } + pose_cov(2, 2) = msg_cov[6 * 5 + 5]; + return pose_cov; +} + +/** + * @brief Eigenvalue decomposition: Symmetric matrix A => eigenvectors in columns of V, + * corresponding eigenvalues in d. + * @param matrix_a Sysmmetric matrix A + * @param matrix_v Eigenvectors in columns of matrix V + * @param vector_d Eigenvalues in vector d + */ +void EigenDecomposition(const Mat3d &matrix_a, + Mat3d &matrix_v, + Vec3d &vector_d); + +inline void Tred2(Mat3d &matrix_v, Vec3d &vector_d, Vec3d &vector_e); + +inline void Tql2(Mat3d &matrix_v, Vec3d &vector_d, Vec3d &vector_e); + +}// math +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_R_MATH_H diff --git a/roborts_localization/localization_node.cpp b/roborts_localization/localization_node.cpp new file mode 100644 index 00000000..06cba6f1 --- /dev/null +++ b/roborts_localization/localization_node.cpp @@ -0,0 +1,343 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "localization_node.h" + +namespace roborts_localization{ + +LocalizationNode::LocalizationNode(std::string name) { + CHECK(Init()) << "Module " << name <<" initialized failed!"; + initialized_ = true; +} + + +bool LocalizationNode::Init() { + + LocalizationConfig localization_config; + localization_config.GetParam(&nh_); + + odom_frame_ = std::move(localization_config.odom_frame_id); + global_frame_ = std::move(localization_config.global_frame_id); + base_frame_ = std::move(localization_config.base_frame_id); + + laser_topic_ = std::move(localization_config.laser_topic_name); + + init_pose_ = {localization_config.initial_pose_x, + localization_config.initial_pose_y, + localization_config.initial_pose_a}; + init_cov_ = {localization_config.initial_cov_xx, + localization_config.initial_cov_yy, + localization_config.initial_cov_aa}; + + transform_tolerance_ = ros::Duration(localization_config.transform_tolerance); + publish_visualize_ = localization_config.publish_visualize; + + tf_broadcaster_ptr_ = std::make_unique(); + tf_listener_ptr_ = std::make_unique(); + + distance_map_pub_ = nh_.advertise("distance_map", 1, true); + particlecloud_pub_ = nh_.advertise("particlecloud", 2, true); + + // Use message filter for time synchronizer (laser scan topic and tf between odom and base frame) + laser_scan_sub_ = std::make_shared>(nh_, laser_topic_, 100); + laser_scan_filter_ = std::make_unique>(*laser_scan_sub_, + *tf_listener_ptr_, + odom_frame_, + 100); + laser_scan_filter_->registerCallback(boost::bind(&LocalizationNode::LaserScanCallback, this, _1)); + + initial_pose_sub_ = nh_.subscribe("initialpose", 2, &LocalizationNode::InitialPoseCallback, this); + pose_pub_ = nh_.advertise("amcl_pose", 2, true); + + amcl_ptr_= std::make_unique(); + amcl_ptr_->GetParamFromRos(&nh_); + amcl_ptr_->Init(init_pose_, init_cov_); + + map_init_ = GetStaticMap(); + laser_init_ = GetLaserPose(); + + return map_init_&&laser_init_; +} + +bool LocalizationNode::GetStaticMap(){ + static_map_srv_ = nh_.serviceClient("static_map"); + ros::service::waitForService("static_map", -1); + nav_msgs::GetMap::Request req; + nav_msgs::GetMap::Response res; + if(static_map_srv_.call(req,res)) { + LOG_INFO << "Received Static Map"; + amcl_ptr_->HandleMapMessage(res.map, init_pose_, init_cov_); + first_map_received_ = true; + return true; + } else{ + LOG_ERROR << "Get static map failed"; + return false; + } +} + +bool LocalizationNode::GetLaserPose() { + auto laser_scan_msg = ros::topic::waitForMessage(laser_topic_); + + Vec3d laser_pose; + laser_pose.setZero(); + GetPoseFromTf(base_frame_, laser_scan_msg->header.frame_id, ros::Time(), laser_pose); + laser_pose[2] = 0; // No need for rotation, or will be error + DLOG_INFO << "Received laser's pose wrt robot: "<< + laser_pose[0] << ", " << + laser_pose[1] << ", " << + laser_pose[2]; + + amcl_ptr_->SetLaserSensorPose(laser_pose); + return true; +} + +void LocalizationNode::InitialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &init_pose_msg) { + + if (init_pose_msg->header.frame_id == "") { + LOG_WARNING << "Received initial pose with empty frame_id."; + } // Only accept initial pose estimates in the global frame + else if (tf_listener_ptr_->resolve(init_pose_msg->header.frame_id) != + tf_listener_ptr_->resolve(global_frame_)) { + LOG_ERROR << "Ignoring initial pose in frame \" " + << init_pose_msg->header.frame_id + << "\"; initial poses must be in the global frame, \"" + << global_frame_; + return; + } + + // In case the client sent a pose estimate in the past, integrate the + // intervening odometric change. + tf::StampedTransform tx_odom; + try { + ros::Time now = ros::Time::now(); + tf_listener_ptr_->waitForTransform(base_frame_, + init_pose_msg->header.stamp, + base_frame_, + now, + odom_frame_, + ros::Duration(0.5)); + tf_listener_ptr_->lookupTransform(base_frame_, + init_pose_msg->header.stamp, + base_frame_, + now, + odom_frame_, tx_odom); + } + catch (tf::TransformException &e) { + tx_odom.setIdentity(); + } + tf::Pose pose_new; + tf::Pose pose_old; + tf::poseMsgToTF(init_pose_msg->pose.pose, pose_old); + pose_new = pose_old * tx_odom; + + // Transform into the global frame + DLOG_INFO << "Setting pose " << ros::Time::now().toSec() << ", " + << pose_new.getOrigin().x() << ", " << pose_new.getOrigin().y(); + + Vec3d init_pose_mean; + Mat3d init_pose_cov; + init_pose_mean.setZero(); + init_pose_cov.setZero(); + double yaw, pitch, roll; + init_pose_mean(0) = pose_new.getOrigin().x(); + init_pose_mean(1) = pose_new.getOrigin().y(); + pose_new.getBasis().getEulerYPR(yaw, pitch, roll); + init_pose_mean(2) = yaw; + init_pose_cov = math::MsgCovarianceToMat3d(init_pose_msg->pose.covariance); + + amcl_ptr_->HandleInitialPoseMessage(init_pose_mean, init_pose_cov); +} + +void LocalizationNode::LaserScanCallback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg_ptr){ + + last_laser_msg_timestamp_ = laser_scan_msg_ptr->header.stamp; + + Vec3d pose_in_odom; + if(!GetPoseFromTf(odom_frame_, base_frame_, last_laser_msg_timestamp_, pose_in_odom)) + { + LOG_ERROR << "Couldn't determine robot's pose"; + return; + } + + double angle_min = 0 , angle_increment = 0; + sensor_msgs::LaserScan laser_scan_msg = *laser_scan_msg_ptr; + TransformLaserscanToBaseFrame(angle_min, angle_increment, laser_scan_msg); + + amcl_ptr_->Update(pose_in_odom, + laser_scan_msg, + angle_min, + angle_increment, + particlecloud_msg_, + hyp_pose_); + + LOG_ERROR_IF(!PublishTf()) << "Publish Tf Error!"; + + if(publish_visualize_){ + PublishVisualize(); + } + +} + +void LocalizationNode::PublishVisualize(){ + + if(pose_pub_.getNumSubscribers() > 0){ + pose_msg_.header.stamp = ros::Time::now(); + pose_msg_.header.frame_id = global_frame_; + pose_msg_.pose.position.x = hyp_pose_.pose_mean[0]; + pose_msg_.pose.position.y = hyp_pose_.pose_mean[1]; + pose_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(hyp_pose_.pose_mean[2]); + pose_pub_.publish(pose_msg_); + } + + if(particlecloud_pub_.getNumSubscribers() > 0){ + particlecloud_msg_.header.stamp = ros::Time::now(); + particlecloud_msg_.header.frame_id = global_frame_; + particlecloud_pub_.publish(particlecloud_msg_); + } + + if(!publish_first_distance_map_) { + distance_map_pub_.publish(amcl_ptr_->GetDistanceMapMsg()); + publish_first_distance_map_ = true; + } +} + +bool LocalizationNode::PublishTf() { + ros::Time transform_expiration = (last_laser_msg_timestamp_ + transform_tolerance_); + if (amcl_ptr_->CheckTfUpdate()) { + // Subtracting base to odom from map to base and send map to odom instead + tf::Stamped odom_to_map; + try { + tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyp_pose_.pose_mean[2]), + tf::Vector3(hyp_pose_.pose_mean[0], + hyp_pose_.pose_mean[1], + 0.0)); + tf::Stamped tmp_tf_stamped(tmp_tf.inverse(), + last_laser_msg_timestamp_, + base_frame_); + this->tf_listener_ptr_->transformPose(odom_frame_, + tmp_tf_stamped, + odom_to_map); + } catch (tf::TransformException &e) { + LOG_ERROR << "Failed to subtract base to odom transform" << e.what(); + return false; + } + + latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), + tf::Point(odom_to_map.getOrigin())); + latest_tf_valid_ = true; + + tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), + transform_expiration, + global_frame_, + odom_frame_); + this->tf_broadcaster_ptr_->sendTransform(tmp_tf_stamped); + sent_first_transform_ = true; + return true; + } else if (latest_tf_valid_) { + // Nothing changed, so we'll just republish the last transform + tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), + transform_expiration, + global_frame_, + odom_frame_); + this->tf_broadcaster_ptr_->sendTransform(tmp_tf_stamped); + return true; + } + else{ + return false; + } +} + +bool LocalizationNode::GetPoseFromTf(const std::string &target_frame, + const std::string &source_frame, + const ros::Time ×tamp, + Vec3d &pose) +{ + tf::Stamped ident(tf::Transform(tf::createIdentityQuaternion(), + tf::Vector3(0, 0, 0)), + timestamp, + source_frame); + tf::Stamped pose_stamp; + try { + this->tf_listener_ptr_->transformPose(target_frame, + ident, + pose_stamp); + } catch (tf::TransformException &e) { + LOG_ERROR << "Couldn't transform from " + << source_frame + << "to " + << target_frame; + return false; + } + + pose.setZero(); + pose[0] = pose_stamp.getOrigin().x(); + pose[1] = pose_stamp.getOrigin().y(); + double yaw,pitch, roll; + pose_stamp.getBasis().getEulerYPR(yaw, pitch, roll); + pose[2] = yaw; + return true; +} + +void LocalizationNode::TransformLaserscanToBaseFrame(double &angle_min, + double &angle_increment, + const sensor_msgs::LaserScan &laser_scan_msg) { + + // To account for lasers that are mounted upside-down, we determine the + // min, max, and increment angles of the laser in the base frame. + // Construct min and max angles of laser, in the base_link frame. + tf::Quaternion q; + q.setRPY(0.0, 0.0, laser_scan_msg.angle_min); + tf::Stamped min_q(q, laser_scan_msg.header.stamp, + laser_scan_msg.header.frame_id); + q.setRPY(0.0, 0.0, laser_scan_msg.angle_min + + laser_scan_msg.angle_increment); + tf::Stamped inc_q(q, laser_scan_msg.header.stamp, + laser_scan_msg.header.frame_id); + + try { + tf_listener_ptr_->transformQuaternion(base_frame_, + min_q, + min_q); + tf_listener_ptr_->transformQuaternion(base_frame_, + inc_q, + inc_q); + } + catch (tf::TransformException &e) { + LOG_WARNING << "Unable to transform min/max laser angles into base frame: " << e.what(); + return; + } + + angle_min = tf::getYaw(min_q); + angle_increment = (tf::getYaw(inc_q) - angle_min); + + // Wrapping angle to [-pi .. pi] + angle_increment = (std::fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI); + +} + +}// roborts_localization + +int main(int argc, char **argv) { + roborts_localization::GLogWrapper glog_wrapper(argv[0]); + ros::init(argc, argv, "localization_node"); + roborts_localization::LocalizationNode localization_node("localization_node"); + ros::AsyncSpinner async_spinner(THREAD_NUM); + async_spinner.start(); + ros::waitForShutdown(); + return 0; +} + diff --git a/roborts_localization/localization_node.h b/roborts_localization/localization_node.h new file mode 100644 index 00000000..ac961656 --- /dev/null +++ b/roborts_localization/localization_node.h @@ -0,0 +1,162 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_LOCALIZATION_LOCALIZATION_NODE_H +#define ROBORTS_LOCALIZATION_LOCALIZATION_NODE_H + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "log.h" +#include "localization_config.h" +#include "amcl/amcl.h" +#include "localization_math.h" +#include "types.h" + +#define THREAD_NUM 4 // ROS SPIN THREAD NUM + +namespace roborts_localization { + +class LocalizationNode { + public: + + /** + * @brief Localization Node construct function + * @param name Node name + */ + LocalizationNode(std::string name); + + /** + * @brief Localization initialization + * @return Returns true if initialize success + */ + bool Init(); + + /** + * @brief Laser scan messages callback function (as main loop in localization node) + * @param laser_scan_msg Laser scan data + */ + void LaserScanCallback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg); + + /** + * @brief Manually initialize localization init_pose + * @param init_pose_msg Init pose (2D Pose Estimate button in RViz) + */ + void InitialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &init_pose_msg); + + /** + * @brief Publish visualize messages + */ + void PublishVisualize(); + + /** + * @brief Publish transform information between odom_frame and global_frame + * @return True if no errors + */ + bool PublishTf(); + + private: + + bool GetPoseFromTf(const std::string &target_frame, + const std::string &source_frame, + const ros::Time ×tamp, + Vec3d &pose); + + bool GetStaticMap(); + + bool GetLaserPose(); + + void TransformLaserscanToBaseFrame(double &angle_min, + double &angle_increment, + const sensor_msgs::LaserScan &laser_scan_msg); + private: + //Mutex + std::mutex mutex_; + + //Algorithm object + std::unique_ptr amcl_ptr_; + + //ROS Node handle + ros::NodeHandle nh_; + + //TF + std::unique_ptr tf_broadcaster_ptr_; + std::unique_ptr tf_listener_ptr_; + + //ROS Subscriber + ros::Subscriber initial_pose_sub_; + ros::Subscriber map_sub_; + ros::Subscriber uwb_pose_sub_; + ros::Subscriber ground_truth_sub_; + std::shared_ptr> laser_scan_sub_; + std::unique_ptr> laser_scan_filter_; + + //ROS Publisher + ros::Publisher pose_pub_; + ros::Publisher particlecloud_pub_; + ros::Publisher distance_map_pub_; + ros::Publisher beam_sample_pub_; + + //ROS Service + ros::ServiceServer random_heading_srv_; + ros::ServiceClient static_map_srv_; + + //Parameters + std::string odom_frame_; + std::string global_frame_; + std::string base_frame_; + std::string laser_topic_; + Vec3d init_pose_; + Vec3d init_cov_; +// bool enable_uwb_; + ros::Duration transform_tolerance_; + bool publish_visualize_; + + //Status + bool initialized_ = false; + bool map_init_ = false; + bool laser_init_ = false; + bool first_map_received_ = false; + bool latest_tf_valid_ = false; + bool sent_first_transform_ = false; + bool publish_first_distance_map_ = false; + + //Data + HypPose hyp_pose_; + geometry_msgs::PoseArray particlecloud_msg_; + geometry_msgs::PoseStamped pose_msg_; + ros::Time last_laser_msg_timestamp_; + tf::Transform latest_tf_; + tf::Stamped latest_odom_pose_; +}; + +}// roborts_localization + +#endif // ROBORTS_LOCALIZATION_LOCALIZATION_NODE_H diff --git a/roborts_localization/log.h b/roborts_localization/log.h new file mode 100644 index 00000000..44ea349e --- /dev/null +++ b/roborts_localization/log.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_LOCALIZATION_LOG_H +#define ROBORTS_LOCALIZATION_LOG_H + +#include +#include "glog/logging.h" +#include "glog/raw_logging.h" +#include + +//#define DLOG_INFO std::cout +//#define DLOG_ERROR std::cout + +// GLog Wrapper +namespace roborts_localization { + +#define LOG_INFO LOG(INFO) +#define LOG_WARNING LOG(WARNING) +#define LOG_ERROR LOG(ERROR) +#define LOG_FATAL LOG(FATAL) + +#define LOG_INFO_IF(condition) LOG_IF(INFO,condition) +#define LOG_WARNING_IF(condition) LOG_IF(WARNING,condition) +#define LOG_ERROR_IF(condition) LOG_IF(ERROR,condition) +#define LOG_FATAL_IF(condition) LOG_IF(FATAL,condition) + +#define LOG_INFO_EVERY(freq) LOG_EVERY_N(INFO, freq) +#define LOG_WARNING_EVERY(freq) LOG_EVERY_N(WARNING, freq) +#define LOG_ERROR_EVERY(freq) LOG_EVERY_N(ERROR, freq) + +#define DLOG_INFO DLOG(INFO) +#define DLOG_WARNING DLOG(WARNING) +#define DLOG_ERROR DLOG(WARNING) + +#define LOG_WARNING_FIRST_N(times) LOG_FIRST_N(WARNING, times) + +class GLogWrapper { + public: + GLogWrapper(char *program) { + google::InitGoogleLogging(program); + FLAGS_stderrthreshold = google::WARNING; + FLAGS_colorlogtostderr = true; + FLAGS_v = 3; + google::InstallFailureSignalHandler(); + } + + ~GLogWrapper() { + google::ShutdownGoogleLogging(); + } +}; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_LOG_H diff --git a/roborts_localization/package.xml b/roborts_localization/package.xml new file mode 100755 index 00000000..e7fdbc2e --- /dev/null +++ b/roborts_localization/package.xml @@ -0,0 +1,28 @@ + + roborts_localization + 1.0.0 + + The roborts localization package provides localization for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + roscpp + rospy + tf + nav_msgs + sensor_msgs + message_filters + + roscpp + rospy + tf + nav_msgs + sensor_msgs + message_filters + + diff --git a/roborts_localization/types.h b/roborts_localization/types.h new file mode 100644 index 00000000..1b515bbb --- /dev/null +++ b/roborts_localization/types.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_LOCALIZATION_TYPES_H +#define ROBORTS_LOCALIZATION_TYPES_H + +#include +#include +#include +#include +#include + +namespace roborts_localization { + +// Use eigen3 as base data structure +using Vec3d = Eigen::Vector3d; +using Vec4d = Eigen::Vector4d; + +using Mat3d = Eigen::Matrix3d; +using Mat2d = Eigen::Matrix2d; + +using MatX2d = Eigen::MatrixX2d; + +}// roborts_localization + +#endif //ROBORTS_LOCALIZATION_TYPES_H diff --git a/roborts_msgs/CMakeLists.txt b/roborts_msgs/CMakeLists.txt new file mode 100644 index 00000000..7d9a3aa9 --- /dev/null +++ b/roborts_msgs/CMakeLists.txt @@ -0,0 +1,48 @@ +project(roborts_msgs) +cmake_minimum_required(VERSION 3.1) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}) +find_package(catkin REQUIRED COMPONENTS + std_msgs + nav_msgs + geometry_msgs + actionlib + message_generation +) + +add_action_files( + DIRECTORY action + FILES + LocalPlanner.action + GlobalPlanner.action + ArmorDetection.action +) + +add_message_files( + DIRECTORY msg + FILES + TwistAccel.msg + GimbalAngle.msg + GimbalRate.msg + ObstacleMsg.msg + ShootInfo.msg + ShootState.msg +) + +add_service_files( + DIRECTORY srv + FILES + ChassisMode.srv + GimbalMode.srv + FricWhl.srv + ShootCmd.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + nav_msgs +) + +catkin_package() diff --git a/roborts_msgs/action/ArmorDetection.action b/roborts_msgs/action/ArmorDetection.action new file mode 100755 index 00000000..05b32d5b --- /dev/null +++ b/roborts_msgs/action/ArmorDetection.action @@ -0,0 +1,13 @@ +#Send a flag to server to control the thread start, pause, restart and stop +#command == 1 start +#command == 2 pause +#command == 3 stop +int32 command +--- +float32 result +--- +#feedback +bool detected +int32 error_code +string error_msg +geometry_msgs/PoseStamped enemy_pos diff --git a/roborts_msgs/action/GlobalPlanner.action b/roborts_msgs/action/GlobalPlanner.action new file mode 100755 index 00000000..d931daa5 --- /dev/null +++ b/roborts_msgs/action/GlobalPlanner.action @@ -0,0 +1,11 @@ +#goal definition +int32 command +geometry_msgs/PoseStamped goal +--- +#result definition RUNNING = 0, SUCCESS = 1, FAILURE = 2 +int32 error_code +--- +#feedback +nav_msgs/Path path +int32 error_code +string error_msg \ No newline at end of file diff --git a/roborts_msgs/action/LocalPlanner.action b/roborts_msgs/action/LocalPlanner.action new file mode 100755 index 00000000..c49181f6 --- /dev/null +++ b/roborts_msgs/action/LocalPlanner.action @@ -0,0 +1,9 @@ +#goal definition +nav_msgs/Path route +--- +#result definition RUNNING = 0, SUCCESS = 1, FAILURE = 2 +int32 error_code +--- +#feedback +int32 error_code +string error_msg \ No newline at end of file diff --git a/roborts_msgs/msg/GimbalAngle.msg b/roborts_msgs/msg/GimbalAngle.msg new file mode 100755 index 00000000..c49cfa54 --- /dev/null +++ b/roborts_msgs/msg/GimbalAngle.msg @@ -0,0 +1,5 @@ +#gimbal feedback angle data +bool yaw_mode +bool pitch_mode +float64 yaw_angle +float64 pitch_angle diff --git a/roborts_msgs/msg/GimbalRate.msg b/roborts_msgs/msg/GimbalRate.msg new file mode 100755 index 00000000..75e54f50 --- /dev/null +++ b/roborts_msgs/msg/GimbalRate.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +float64 pitch_rate +float64 yaw_rate diff --git a/roborts_msgs/msg/ObstacleMsg.msg b/roborts_msgs/msg/ObstacleMsg.msg new file mode 100755 index 00000000..00d48c91 --- /dev/null +++ b/roborts_msgs/msg/ObstacleMsg.msg @@ -0,0 +1,30 @@ +# Message that contains a list of polygon shaped obstacles. +# Special types: +# Polygon with 1 vertex: Point obstacle +# Polygon with 2 vertices: Line obstacle +# Polygon with more than 2 vertices: First and last points are assumed to be connected +# +# If optional properties (orientaions or velocities) are provided, +# each container size must match the number of obstacles +# otherwise let them empty. + + +std_msgs/Header header + +# Actual obstacle positions (polygon descriptions) +geometry_msgs/PolygonStamped[] obstacles + +# Obstacle IDs [optional] +# Specify IDs in order to provide (temporal) relationships +# between obstacles among multiple messages. +# If provided it must be size(ids) = size(obstacles) +uint32[] ids + +# Individual orientations (centroid) [optional] +# If provided it must be size(orientations) = size(obstacles) +geometry_msgs/QuaternionStamped[] orientations + +# Individual velocities (centroid) [optional] +# If provided it must be size(velocities) = size(obstacles) +geometry_msgs/TwistWithCovariance[] velocities + diff --git a/roborts_msgs/msg/ShootInfo.msg b/roborts_msgs/msg/ShootInfo.msg new file mode 100644 index 00000000..bab9345a --- /dev/null +++ b/roborts_msgs/msg/ShootInfo.msg @@ -0,0 +1,10 @@ +Header header + +#Remain bullets number +int16 remain_bullet + +#Bullets fired number +int16 sent_bullet + +#Friction running status +bool fric_wheel_run \ No newline at end of file diff --git a/roborts_msgs/msg/ShootState.msg b/roborts_msgs/msg/ShootState.msg new file mode 100755 index 00000000..d1f0ba30 --- /dev/null +++ b/roborts_msgs/msg/ShootState.msg @@ -0,0 +1,4 @@ +int32 single_shoot +int32 continue_shoot +int32 run_friction_whell +int32 friction_whell_speed \ No newline at end of file diff --git a/roborts_msgs/msg/TwistAccel.msg b/roborts_msgs/msg/TwistAccel.msg new file mode 100644 index 00000000..fcd481d1 --- /dev/null +++ b/roborts_msgs/msg/TwistAccel.msg @@ -0,0 +1,2 @@ +geometry_msgs/Twist twist +geometry_msgs/Accel accel \ No newline at end of file diff --git a/roborts_msgs/package.xml b/roborts_msgs/package.xml new file mode 100755 index 00000000..b49a9a5c --- /dev/null +++ b/roborts_msgs/package.xml @@ -0,0 +1,32 @@ + + roborts_msgs + 1.0.0 + + The roborts msgs package provides all the messages for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + message_generation + + + actionlib + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + message_runtime + + + + + diff --git a/roborts_msgs/srv/ChassisMode.srv b/roborts_msgs/srv/ChassisMode.srv new file mode 100644 index 00000000..f753b9a9 --- /dev/null +++ b/roborts_msgs/srv/ChassisMode.srv @@ -0,0 +1,3 @@ +uint8 chassis_mode +--- +bool received \ No newline at end of file diff --git a/roborts_msgs/srv/FricWhl.srv b/roborts_msgs/srv/FricWhl.srv new file mode 100644 index 00000000..c8b4e069 --- /dev/null +++ b/roborts_msgs/srv/FricWhl.srv @@ -0,0 +1,3 @@ +bool open +--- +bool received diff --git a/roborts_msgs/srv/GimbalMode.srv b/roborts_msgs/srv/GimbalMode.srv new file mode 100644 index 00000000..a818c3b1 --- /dev/null +++ b/roborts_msgs/srv/GimbalMode.srv @@ -0,0 +1,3 @@ +uint8 gimbal_mode +--- +bool received diff --git a/roborts_msgs/srv/ShootCmd.srv b/roborts_msgs/srv/ShootCmd.srv new file mode 100644 index 00000000..67e5c384 --- /dev/null +++ b/roborts_msgs/srv/ShootCmd.srv @@ -0,0 +1,4 @@ +uint8 mode +uint8 number +--- +bool received diff --git a/roborts_planning/CMakeLists.txt b/roborts_planning/CMakeLists.txt new file mode 100755 index 00000000..cf09874a --- /dev/null +++ b/roborts_planning/CMakeLists.txt @@ -0,0 +1,34 @@ +project(roborts_planning) +cmake_minimum_required(VERSION 3.1) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) +set(CMAKE_BUILD_TYPE Release) + +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + pcl_conversions + pcl_ros + nav_msgs + geometry_msgs + actionlib + roborts_common + roborts_msgs + interactive_markers + roborts_costmap + ) + +find_package(G2O REQUIRED) +find_package(SUITESPARSE REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL 1.7 REQUIRED) +find_package(ProtoBuf REQUIRED) +find_package(Boost REQUIRED) + +catkin_package() + +add_subdirectory(global_planner) +add_subdirectory(local_planner) + + diff --git a/roborts_planning/cmake_module/FindEigen3.cmake b/roborts_planning/cmake_module/FindEigen3.cmake new file mode 100755 index 00000000..239c1bbe --- /dev/null +++ b/roborts_planning/cmake_module/FindEigen3.cmake @@ -0,0 +1,86 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/roborts_planning/cmake_module/FindG2O.cmake b/roborts_planning/cmake_module/FindG2O.cmake new file mode 100755 index 00000000..b2670d39 --- /dev/null +++ b/roborts_planning/cmake_module/FindG2O.cmake @@ -0,0 +1,97 @@ +# Locate the g2o libraries +# A general framework for graph optimization. +# +# This module defines +# G2O_FOUND, if false, do not try to link against g2o +# G2O_LIBRARIES, path to the libg2o +# G2O_INCLUDE_DIR, where to find the g2o header files +# +# Niko Suenderhauf +# Adapted by Felix Endres + +IF(UNIX) + + #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) + # in cache already + # SET(G2O_FIND_QUIETLY TRUE) + #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) + + MESSAGE(STATUS "Searching for g2o ...") + FIND_PATH(G2O_INCLUDE_DIR + NAMES core math_groups types + PATHS /usr/local /usr + PATH_SUFFIXES include/g2o include) + + IF (G2O_INCLUDE_DIR) + MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}") + ENDIF (G2O_INCLUDE_DIR) + + FIND_LIBRARY(G2O_CORE_LIB + NAMES g2o_core g2o_core_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_STUFF_LIB + NAMES g2o_stuff g2o_stuff_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB + NAMES g2o_types_slam2d g2o_types_slam2d_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB + NAMES g2o_types_slam3d g2o_types_slam3d_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB + NAMES g2o_solver_cholmod g2o_solver_cholmod_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_SOLVER_PCG_LIB + NAMES g2o_solver_pcg g2o_solver_pcg_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB + NAMES g2o_solver_csparse g2o_solver_csparse_rd + PATHS /usr/local /usr + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_INCREMENTAL_LIB + NAMES g2o_incremental g2o_incremental_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB + NAMES g2o_csparse_extension g2o_csparse_extension_rd + PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} + PATH_SUFFIXES lib) + + SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB} + ${G2O_CORE_LIB} + ${G2O_STUFF_LIB} + ${G2O_TYPES_SLAM2D_LIB} + ${G2O_TYPES_SLAM3D_LIB} + ${G2O_SOLVER_CHOLMOD_LIB} + ${G2O_SOLVER_PCG_LIB} + ${G2O_SOLVER_CSPARSE_LIB} + ${G2O_INCREMENTAL_LIB} + ) + + IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) + SET(G2O_FOUND "YES") + IF(NOT G2O_FIND_QUIETLY) + MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}") + ENDIF(NOT G2O_FIND_QUIETLY) + ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR) + IF(NOT G2O_LIBRARIES) + IF(G2O_FIND_REQUIRED) + message(FATAL_ERROR "Could not find libg2o!") + ENDIF(G2O_FIND_REQUIRED) + ENDIF(NOT G2O_LIBRARIES) + + IF(NOT G2O_INCLUDE_DIR) + IF(G2O_FIND_REQUIRED) + message(FATAL_ERROR "Could not find g2o include directory!") + ENDIF(G2O_FIND_REQUIRED) + ENDIF(NOT G2O_INCLUDE_DIR) + ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) + +ENDIF(UNIX) + diff --git a/roborts_planning/cmake_module/FindProtoBuf.cmake b/roborts_planning/cmake_module/FindProtoBuf.cmake new file mode 100755 index 00000000..3462e7be --- /dev/null +++ b/roborts_planning/cmake_module/FindProtoBuf.cmake @@ -0,0 +1,84 @@ +# Finds Google Protocol Buffers library and compilers and extends +# the standard cmake script + +find_package(Protobuf REQUIRED) +list(APPEND RRTS_INCLUDE_DIRS PUBLIC ${PROTOBUF_INCLUDE_DIR}) +list(APPEND RRTS_LINKER_LIBS PUBLIC ${PROTOBUF_LIBRARIES}) + +# As of Ubuntu 14.04 protoc is no longer a part of libprotobuf-dev package +# and should be installed separately as in: sudo apt-get install protobuf-compiler +if(EXISTS ${PROTOBUF_PROTOC_EXECUTABLE}) + message(STATUS "Found PROTOBUF Compiler: ${PROTOBUF_PROTOC_EXECUTABLE}") +else() + message(FATAL_ERROR "Could not find PROTOBUF Compiler") +endif() + +if (PROTOBUF_FOUND) + message ("protobuf ${PROTOBUF_LIBRARIES} found") +else () + message (FATAL_ERROR "Cannot find protobuf") +endif () + +# place where to generate protobuf sources +#set(proto_gen_folder "${PROJECT_BINARY_DIR}/include/caffe/proto") +#include_directories("${PROJECT_BINARY_DIR}/include") + +set(PROTOBUF_GENERATE_CPP_APPEND_PATH TRUE) + +################################################################################################ +# Modification of standard 'protobuf_generate_cpp()' with output dir parameter +# Usage: +# caffe_protobuf_generate_cpp_py( ) +function(rrts_protobuf_generate_cpp output_dir proto_srcs proto_hdrs) + if(NOT ARGN) + message(SEND_ERROR "Error: rrts_protobuf_generate_cpp() called without any proto files") + return() + endif() + + if(PROTOBUF_GENERATE_CPP_APPEND_PATH) + # Create an include path for each file specified + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(abs_path ${abs_fil} PATH) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + else() + set(_protoc_include -I ${CMAKE_CURRENT_SOURCE_DIR}) + endif() + + if(DEFINED PROTOBUF_IMPORT_DIRS) + foreach(dir ${PROTOBUF_IMPORT_DIRS}) + get_filename_component(abs_path ${dir} ABSOLUTE) + list(FIND _protoc_include ${abs_path} _contains_already) + if(${_contains_already} EQUAL -1) + list(APPEND _protoc_include -I ${abs_path}) + endif() + endforeach() + endif() + + set(${proto_srcs}) + set(${proto_hdrs}) + foreach(fil ${ARGN}) + get_filename_component(abs_fil ${fil} ABSOLUTE) + get_filename_component(fil_we ${fil} NAME_WE) + + list(APPEND ${proto_srcs} "${output_dir}/${fil_we}.pb.cc") + list(APPEND ${proto_hdrs} "${output_dir}/${fil_we}.pb.h") + + add_custom_command( + OUTPUT "${output_dir}/${fil_we}.pb.cpp" + "${output_dir}/${fil_we}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${output_dir} ${_protoc_include} ${abs_fil} + DEPENDS ${abs_fil} + COMMENT "Running C++ protocol buffer compiler on ${fil}" + VERBATIM ) + endforeach() + + set_source_files_properties(${${proto_srcs}} ${${proto_hdrs}} PROPERTIES GENERATED TRUE) + set(${proto_srcs} ${${proto_srcs}} PARENT_SCOPE) + set(${proto_hdrs} ${${proto_hdrs}} PARENT_SCOPE) +endfunction() diff --git a/roborts_planning/cmake_module/FindSUITESPARSE.cmake b/roborts_planning/cmake_module/FindSUITESPARSE.cmake new file mode 100755 index 00000000..101b79bf --- /dev/null +++ b/roborts_planning/cmake_module/FindSUITESPARSE.cmake @@ -0,0 +1,133 @@ +# - Try to find SUITESPARSE +# Once done this will define +# +# SUITESPARSE_FOUND - system has SUITESPARSE +# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory +# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE +# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package) +# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package) +# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs +# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs +# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly + +IF (SUITESPARSE_INCLUDE_DIRS) + # Already in cache, be silent + SET(SUITESPARSE_FIND_QUIETLY TRUE) +ENDIF (SUITESPARSE_INCLUDE_DIRS) + +if( WIN32 ) + # Find cholmod part of the suitesparse library collection + + FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h + PATHS "C:\\libs\\win32\\SuiteSparse\\Include" ) + + # Add cholmod include directory to collection include directories + IF ( CHOLMOD_INCLUDE_DIR ) + list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) + ENDIF( CHOLMOD_INCLUDE_DIR ) + + + # find path suitesparse library + FIND_PATH( SUITESPARSE_LIBRARY_DIRS + amd.lib + PATHS "C:\\libs\\win32\\SuiteSparse\\libs" ) + + # if we found the library, add it to the defined libraries + IF ( SUITESPARSE_LIBRARY_DIRS ) + list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd ) + ENDIF( SUITESPARSE_LIBRARY_DIRS ) + +else( WIN32 ) + IF(APPLE) + FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h + PATHS /opt/local/include/ufsparse + /usr/local/include ) + + FIND_PATH( SUITESPARSE_LIBRARY_DIR + NAMES libcholmod.a + PATHS /opt/local/lib + /usr/local/lib ) + ELSE(APPLE) + FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h + PATHS /usr/local/include + /usr/include + /usr/include/suitesparse/ + ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod + PATH_SUFFIXES cholmod/ CHOLMOD/ ) + + FIND_PATH( SUITESPARSE_LIBRARY_DIR + NAMES libcholmod.so libcholmod.a + PATHS /usr/lib + /usr/lib64 + /usr/lib/x86_64-linux-gnu + /usr/lib/i386-linux-gnu + /usr/local/lib + /usr/lib/arm-linux-gnueabihf/ + /usr/lib/aarch64-linux-gnu/ + /usr/lib/arm-linux-gnueabi/ + /usr/lib/arm-linux-gnu) + ENDIF(APPLE) + + # Add cholmod include directory to collection include directories + IF ( CHOLMOD_INCLUDE_DIR ) + list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) + ENDIF( CHOLMOD_INCLUDE_DIR ) + + # if we found the library, add it to the defined libraries + IF ( SUITESPARSE_LIBRARY_DIR ) + + list ( APPEND SUITESPARSE_LIBRARIES amd) + list ( APPEND SUITESPARSE_LIBRARIES btf) + list ( APPEND SUITESPARSE_LIBRARIES camd) + list ( APPEND SUITESPARSE_LIBRARIES ccolamd) + list ( APPEND SUITESPARSE_LIBRARIES cholmod) + list ( APPEND SUITESPARSE_LIBRARIES colamd) + # list ( APPEND SUITESPARSE_LIBRARIES csparse) + list ( APPEND SUITESPARSE_LIBRARIES cxsparse) + list ( APPEND SUITESPARSE_LIBRARIES klu) + # list ( APPEND SUITESPARSE_LIBRARIES spqr) + list ( APPEND SUITESPARSE_LIBRARIES umfpack) + + IF (APPLE) + list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig) + ENDIF (APPLE) + + # Metis and spqr are optional + FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY + NAMES metis + PATHS ${SUITESPARSE_LIBRARY_DIR} ) + IF (SUITESPARSE_METIS_LIBRARY) + list ( APPEND SUITESPARSE_LIBRARIES metis) + ENDIF(SUITESPARSE_METIS_LIBRARY) + + if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp") + SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid") + else() + SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid") + endif() + + if(SUITESPARSE_SPQR_VALID) + FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY + NAMES spqr + PATHS ${SUITESPARSE_LIBRARY_DIR} ) + IF (SUITESPARSE_SPQR_LIBRARY) + list ( APPEND SUITESPARSE_LIBRARIES spqr) + ENDIF (SUITESPARSE_SPQR_LIBRARY) + endif() + + ENDIF( SUITESPARSE_LIBRARY_DIR ) + +endif( WIN32 ) + + +IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) + IF(WIN32) + list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig ) + ENDIF(WIN32) + SET(SUITESPARSE_FOUND TRUE) + MESSAGE(STATUS "Found SuiteSparse") +ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) + SET( SUITESPARSE_FOUND FALSE ) + MESSAGE(FATAL_ERROR "Unable to find SuiteSparse") +ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) + diff --git a/roborts_planning/global_planner/CMakeLists.txt b/roborts_planning/global_planner/CMakeLists.txt new file mode 100755 index 00000000..e194c7c7 --- /dev/null +++ b/roborts_planning/global_planner/CMakeLists.txt @@ -0,0 +1,35 @@ +project(global_planner) + +add_subdirectory(a_star_planner) +set(CMAKE_BUILD_TYPE Release) +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto GlobalPlannerProtoSrc GlobalPlannerProtoHds ${ProtoFiles}) + +include_directories(${catkin_INCLUDE_DIRS}) + +add_executable(${PROJECT_NAME}_node + ${GlobalPlannerProtoSrc} + ${GlobalPlannerProtoHds} + global_planner_node.cpp +) +target_link_libraries(${PROJECT_NAME}_node + PRIVATE + planning::global_planner::a_star_planner + roborts_costmap + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${GLOG_LIBRARY} +) +add_dependencies(${PROJECT_NAME}_node + roborts_msgs_generate_messages) + +add_executable(${PROJECT_NAME}_test + global_planner_test.cpp) +target_link_libraries(${PROJECT_NAME}_test + PRIVATE + ${catkin_LIBRARIES} +) +add_dependencies(${PROJECT_NAME}_test + roborts_msgs_generate_messages) + + diff --git a/roborts_planning/global_planner/a_star_planner/CMakeLists.txt b/roborts_planning/global_planner/a_star_planner/CMakeLists.txt new file mode 100755 index 00000000..46085651 --- /dev/null +++ b/roborts_planning/global_planner/a_star_planner/CMakeLists.txt @@ -0,0 +1,18 @@ +project(a_star_planner) +set(CMAKE_BUILD_TYPE Release) +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto AStarPlannerConfigProtoSrc AStarPlannerConfigProtoHds ${ProtoFiles}) + +include_directories(${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + SHARED + ${AStarPlannerConfigProtoSrc} + ${AStarPlannerConfigProtoHds} + a_star_planner.cpp + ) +target_link_libraries(${PROJECT_NAME} + PUBLIC + roborts_costmap +) +add_library(planning::global_planner::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) diff --git a/roborts_planning/global_planner/a_star_planner/a_star_planner.cpp b/roborts_planning/global_planner/a_star_planner/a_star_planner.cpp new file mode 100755 index 00000000..4f29deb3 --- /dev/null +++ b/roborts_planning/global_planner/a_star_planner/a_star_planner.cpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "a_star_planner.h" + +namespace roborts_global_planner{ + +using roborts_common::ErrorCode; +using roborts_common::ErrorInfo; + +AStarPlanner::AStarPlanner(CostmapPtr costmap_ptr) : + GlobalPlannerBase::GlobalPlannerBase(costmap_ptr), + gridmap_width_(costmap_ptr_->GetCostMap()->GetSizeXCell()), + gridmap_height_(costmap_ptr_->GetCostMap()->GetSizeYCell()), + cost_(costmap_ptr_->GetCostMap()->GetCharMap()) { + + AStarPlannerConfig a_star_planner_config; + std::string full_path = ros::package::getPath("roborts_planning") + "/global_planner/a_star_planner/"\ + "config/a_star_planner_config.prototxt"; + + if (!roborts_common::ReadProtoFromTextFile(full_path.c_str(), + &a_star_planner_config)) { + ROS_ERROR("Cannot load a star planner protobuf configuration file."); + } + // AStarPlanner param config + heuristic_factor_ = a_star_planner_config.heuristic_factor(); + inaccessible_cost_ = a_star_planner_config.inaccessible_cost(); + goal_search_tolerance_ = a_star_planner_config.goal_search_tolerance()/costmap_ptr->GetCostMap()->GetResolution(); +} + +AStarPlanner::~AStarPlanner(){ + cost_ = nullptr; +} + +ErrorInfo AStarPlanner::Plan(const geometry_msgs::PoseStamped &start, + const geometry_msgs::PoseStamped &goal, + std::vector &path) { + + unsigned int start_x, start_y, goal_x, goal_y, tmp_goal_x, tmp_goal_y; + unsigned int valid_goal[2]; + unsigned int shortest_dist = std::numeric_limits::max(); + bool goal_valid = false; + + if (!costmap_ptr_->GetCostMap()->World2Map(start.pose.position.x, + start.pose.position.y, + start_x, + start_y)) { + ROS_WARN("Failed to transform start pose from map frame to costmap frame"); + return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR, + "Start pose can't be transformed to costmap frame."); + } + if (!costmap_ptr_->GetCostMap()->World2Map(goal.pose.position.x, + goal.pose.position.y, + goal_x, + goal_y)) { + ROS_WARN("Failed to transform goal pose from map frame to costmap frame"); + return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR, + "Goal pose can't be transformed to costmap frame."); + } + if (costmap_ptr_->GetCostMap()->GetCost(goal_x,goal_y)GetCostMap()->GetCost(tmp_goal_x, tmp_goal_y); + unsigned int dist = abs(goal_x - tmp_goal_x) + abs(goal_y - tmp_goal_y); + if (cost < inaccessible_cost_ && dist < shortest_dist ) { + shortest_dist = dist; + valid_goal[0] = tmp_goal_x; + valid_goal[1] = tmp_goal_y; + goal_valid = true; + } + tmp_goal_x += 1; + } + tmp_goal_y += 1; + } + } + ErrorInfo error_info; + if (!goal_valid){ + error_info=ErrorInfo(ErrorCode::GP_GOAL_INVALID_ERROR); + path.clear(); + } + else{ + unsigned int start_index, goal_index; + start_index = costmap_ptr_->GetCostMap()->GetIndex(start_x, start_y); + goal_index = costmap_ptr_->GetCostMap()->GetIndex(valid_goal[0], valid_goal[1]); + + costmap_ptr_->GetCostMap()->SetCost(start_x, start_y,roborts_costmap::FREE_SPACE); + + if(start_index == goal_index){ + error_info=ErrorInfo::OK(); + path.clear(); + path.push_back(start); + path.push_back(goal); + } + else{ + error_info = SearchPath(start_index, goal_index, path); + if ( error_info.IsOK() ){ + path.back().pose.orientation = goal.pose.orientation; + path.back().pose.position.z = goal.pose.position.z; + } + } + + } + + + return error_info; +} + +ErrorInfo AStarPlanner::SearchPath(const int &start_index, + const int &goal_index, + std::vector &path) { + + g_score_.clear(); + f_score_.clear(); + parent_.clear(); + state_.clear(); + gridmap_width_ = costmap_ptr_->GetCostMap()->GetSizeXCell(); + gridmap_height_ = costmap_ptr_->GetCostMap()->GetSizeYCell(); + ROS_INFO("Search in a map %d", gridmap_width_*gridmap_height_); + cost_ = costmap_ptr_->GetCostMap()->GetCharMap(); + g_score_.resize(gridmap_height_ * gridmap_width_, std::numeric_limits::max()); + f_score_.resize(gridmap_height_ * gridmap_width_, std::numeric_limits::max()); + parent_.resize(gridmap_height_ * gridmap_width_, std::numeric_limits::max()); + state_.resize(gridmap_height_ * gridmap_width_, SearchState::NOT_HANDLED); + + std::priority_queue, Compare> openlist; + g_score_.at(start_index) = 0; + openlist.push(start_index); + + std::vector neighbors_index; + int current_index, move_cost, h_score, count = 0; + + while (!openlist.empty()) { + current_index = openlist.top(); + openlist.pop(); + state_.at(current_index) = SearchState::CLOSED; + + if (current_index == goal_index) { + ROS_INFO("Search takes %d cycle counts", count); + break; + } + + GetNineNeighbors(current_index, neighbors_index); + + for (auto neighbor_index : neighbors_index) { + + if (neighbor_index < 0 || + neighbor_index >= gridmap_height_ * gridmap_width_) { + continue; + } + + if (cost_[neighbor_index] >= inaccessible_cost_ || + state_.at(neighbor_index) == SearchState::CLOSED) { + continue; + } + + GetMoveCost(current_index, neighbor_index, move_cost); + + if (g_score_.at(neighbor_index) > g_score_.at(current_index) + move_cost + cost_[neighbor_index]) { + + g_score_.at(neighbor_index) = g_score_.at(current_index) + move_cost + cost_[neighbor_index]; + parent_.at(neighbor_index) = current_index; + + if (state_.at(neighbor_index) == SearchState::NOT_HANDLED) { + GetManhattanDistance(neighbor_index, goal_index, h_score); + f_score_.at(neighbor_index) = g_score_.at(neighbor_index) + h_score; + openlist.push(neighbor_index); + state_.at(neighbor_index) = SearchState::OPEN; + } + } + } + count++; + } + + if (current_index != goal_index) { + ROS_WARN("Global planner can't search the valid path!"); + return ErrorInfo(ErrorCode::GP_PATH_SEARCH_ERROR, "Valid global path not found."); + } + + unsigned int iter_index = current_index, iter_x, iter_y; + + geometry_msgs::PoseStamped iter_pos; + iter_pos.pose.orientation.w = 1; + iter_pos.header.frame_id = "map"; + path.clear(); + costmap_ptr_->GetCostMap()->Index2Cells(iter_index, iter_x, iter_y); + costmap_ptr_->GetCostMap()->Map2World(iter_x, iter_y, iter_pos.pose.position.x, iter_pos.pose.position.y); + path.push_back(iter_pos); + + while (iter_index != start_index) { + iter_index = parent_.at(iter_index); +// if(cost_[iter_index]>= inaccessible_cost_){ +// LOG_INFO<<"Cost changes through planning for"<< static_cast(cost_[iter_index]); +// } + costmap_ptr_->GetCostMap()->Index2Cells(iter_index, iter_x, iter_y); + costmap_ptr_->GetCostMap()->Map2World(iter_x, iter_y, iter_pos.pose.position.x, iter_pos.pose.position.y); + path.push_back(iter_pos); + } + + std::reverse(path.begin(),path.end()); + + return ErrorInfo(ErrorCode::OK); + +} + +ErrorInfo AStarPlanner::GetMoveCost(const int ¤t_index, + const int &neighbor_index, + int &move_cost) const { + if (abs(neighbor_index - current_index) == 1 || + abs(neighbor_index - current_index) == gridmap_width_) { + move_cost = 10; + } else if (abs(neighbor_index - current_index) == (gridmap_width_ + 1) || + abs(neighbor_index - current_index) == (gridmap_width_ - 1)) { + move_cost = 14; + } else { + return ErrorInfo(ErrorCode::GP_MOVE_COST_ERROR, + "Move cost can't be calculated cause current neighbor index is not accessible"); + } + return ErrorInfo(ErrorCode::OK); +} + +void AStarPlanner::GetManhattanDistance(const int &index1, const int &index2, int &manhattan_distance) const { + manhattan_distance = heuristic_factor_* 10 * (abs(index1 / gridmap_width_ - index2 / gridmap_width_) + + abs(index1 % gridmap_width_ - index2 % gridmap_width_)); +} + +void AStarPlanner::GetNineNeighbors(const int ¤t_index, std::vector &neighbors_index) const { + neighbors_index.clear(); + if(current_index - gridmap_width_ >= 0){ + neighbors_index.push_back(current_index - gridmap_width_); //up + } + if(current_index - gridmap_width_ - 1 >= 0 && (current_index - gridmap_width_ - 1 + 1) % gridmap_width_!= 0){ + neighbors_index.push_back(current_index - gridmap_width_ - 1); //left_up + } + if(current_index - 1 >= 0 && (current_index - 1 + 1) % gridmap_width_!= 0){ + neighbors_index.push_back(current_index - 1); //left + } + if(current_index + gridmap_width_ - 1 < gridmap_width_* gridmap_height_ + && (current_index + gridmap_width_ - 1 + 1) % gridmap_width_!= 0){ + neighbors_index.push_back(current_index + gridmap_width_ - 1); //left_down + } + if(current_index + gridmap_width_ < gridmap_width_* gridmap_height_){ + neighbors_index.push_back(current_index + gridmap_width_); //down + } + if(current_index + gridmap_width_ + 1 < gridmap_width_* gridmap_height_ + && (current_index + gridmap_width_ + 1 ) % gridmap_width_!= 0){ + neighbors_index.push_back(current_index + gridmap_width_ + 1); //right_down + } + if(current_index + 1 < gridmap_width_* gridmap_height_ + && (current_index + 1 ) % gridmap_width_!= 0) { + neighbors_index.push_back(current_index + 1); //right + } + if(current_index - gridmap_width_ + 1 >= 0 + && (current_index - gridmap_width_ + 1 ) % gridmap_width_!= 0) { + neighbors_index.push_back(current_index - gridmap_width_ + 1); //right_up + } +} + +} //namespace roborts_global_planner diff --git a/roborts_planning/global_planner/a_star_planner/a_star_planner.h b/roborts_planning/global_planner/a_star_planner/a_star_planner.h new file mode 100755 index 00000000..72cda8de --- /dev/null +++ b/roborts_planning/global_planner/a_star_planner/a_star_planner.h @@ -0,0 +1,143 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_A_STAR_PLANNER_H +#define ROBORTS_PLANNING_GLOBAL_PLANNER_A_STAR_PLANNER_H + +#include +#include + +#include "proto/a_star_planner_config.pb.h" + +#include "alg_factory/algorithm_factory.h" +#include "state/error_code.h" +#include "costmap/costmap_interface.h" + +#include "../global_planner_base.h" + +namespace roborts_global_planner{ + +/** + * @brief Global planner alogorithm class for A star under the representation of costmap + */ +class AStarPlanner : public GlobalPlannerBase { + + public: + /** + * @brief Constructor of A star planner, set the costmap pointer and relevant costmap size. + * @param costmap_ptr The shared pointer of costmap interface + */ + AStarPlanner(CostmapPtr costmap_ptr); + virtual ~AStarPlanner(); + /** + * @brief Main Plan function(override the base-class function) + * @param start Start pose input + * @param goal Goal pose input + * @param path Global plan path output + * @return ErrorInfo which is OK if succeed + */ + roborts_common::ErrorInfo Plan(const geometry_msgs::PoseStamped &start, + const geometry_msgs::PoseStamped &goal, + std::vector &path); + + private: + /** + * @brief State enumerate for the cell. + */ + enum SearchState { + NOT_HANDLED, /**< The cell is not handled.*/ + OPEN, /**< The cell is in open priority queue.*/ + CLOSED /**< The cell is in close queue.*/ + }; + /** + * @brief Plan based on 1D Costmap list. Input the index in the costmap and get the plan path. + * @param start_index start pose index in the 1D costmap list + * @param goal_index goal pose index in the 1D costmap list + * @param path plan path output + * @return ErrorInfo which is OK if succeed + */ + roborts_common::ErrorInfo SearchPath(const int &start_index, + const int &goal_index, + std::vector &path); + /** + * @brief Calculate the cost for the diagonal or parallel movement. + * @param current_index Index of the current cell as input + * @param neighbor_index Index of the neighbor cell as input + * @param move_cost Movement cost as output + * @return ErrorInfo which is OK if succeed + */ + roborts_common::ErrorInfo GetMoveCost(const int ¤t_index, + const int &neighbor_index, + int &move_cost) const; + /** + * @brief Calculate the Manhattan distance between two cell index used as the heuristic function of A star algorithm. + * @param index1 Index of the first cell as input + * @param index2 Index of the second cell as input + * @param manhattan_distance The Manhattan distance as output + */ + void GetManhattanDistance(const int &index1, + const int &index2, + int &manhattan_distance) const; + /** + * @brief Get the index of nine neighbor cell from the current cell + * @param current_index Index of the current cell as input + * @param neighbors_index Index of the neighbor cells as out + */ + void GetNineNeighbors(const int ¤t_index, + std::vector &neighbors_index) const; + + /** + * @brief Used for priority queue compare process. + */ + struct Compare { + bool operator()(const int &index1, const int &index2) { + return AStarPlanner::f_score_.at(index1) > AStarPlanner::f_score_.at(index2); + } + }; + + //! heuristic_factor_ + float heuristic_factor_; + //! inaccessible_cost + unsigned int inaccessible_cost_; + //! goal_search_tolerance + unsigned int goal_search_tolerance_; + //! gridmap height size + unsigned int gridmap_height_; + //! gridmap height width + unsigned int gridmap_width_; + //! gridmap cost array + unsigned char *cost_; + //! search algorithm related f score, f_score = g_score + heuristic_cost_estimate + static std::vector f_score_; + //! search algorithm related g score, which refers to the score from start cell to current cell + std::vector g_score_; + //! vector that indicates the parent cell index of each cell + std::vector parent_; + //! vector that indicates the state of each cell + std::vector state_; + + +}; + +std::vector AStarPlanner::f_score_; +roborts_common::REGISTER_ALGORITHM(GlobalPlannerBase, + "a_star_planner", + AStarPlanner, + std::shared_ptr); + +} //namespace roborts_global_planner + +#endif // ROBORTS_PLANNING_GLOBAL_PLANNER_A_STAR_PLANNER_H diff --git a/roborts_planning/global_planner/a_star_planner/config/a_star_planner_config.prototxt b/roborts_planning/global_planner/a_star_planner/config/a_star_planner_config.prototxt new file mode 100755 index 00000000..9cc1f4ca --- /dev/null +++ b/roborts_planning/global_planner/a_star_planner/config/a_star_planner_config.prototxt @@ -0,0 +1,3 @@ +inaccessible_cost: 253 +heuristic_factor: 1.0 +goal_search_tolerance: 0.45 \ No newline at end of file diff --git a/roborts_planning/global_planner/a_star_planner/proto/a_star_planner_config.proto b/roborts_planning/global_planner/a_star_planner/proto/a_star_planner_config.proto new file mode 100755 index 00000000..dd480057 --- /dev/null +++ b/roborts_planning/global_planner/a_star_planner/proto/a_star_planner_config.proto @@ -0,0 +1,8 @@ +syntax = "proto2"; +package roborts_global_planner; + +message AStarPlannerConfig { + optional uint32 inaccessible_cost = 1 [default = 253]; + optional float heuristic_factor = 2 [default = 1.0]; + optional float goal_search_tolerance = 3 [default = 0.25]; +} diff --git a/roborts_planning/global_planner/config/global_planner_config.prototxt b/roborts_planning/global_planner/config/global_planner_config.prototxt new file mode 100755 index 00000000..39a04d4b --- /dev/null +++ b/roborts_planning/global_planner/config/global_planner_config.prototxt @@ -0,0 +1,6 @@ + name: "a_star_planner" + selected_algorithm: "a_star_planner" + frequency: 3 + max_retries: 5 + goal_distance_tolerance: 0.15 + goal_angle_tolerance: 0.15 diff --git a/roborts_planning/global_planner/global_planner_algorithms.h b/roborts_planning/global_planner/global_planner_algorithms.h new file mode 100755 index 00000000..60bd2fa2 --- /dev/null +++ b/roborts_planning/global_planner/global_planner_algorithms.h @@ -0,0 +1,22 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_ALGORITHM_H +#define ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_ALGORITHM_H + +#include "a_star_planner/a_star_planner.h" + +#endif // ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_ALGORITHM_H \ No newline at end of file diff --git a/roborts_planning/global_planner/global_planner_base.h b/roborts_planning/global_planner/global_planner_base.h new file mode 100755 index 00000000..4e76dbb4 --- /dev/null +++ b/roborts_planning/global_planner/global_planner_base.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_BASE_H +#define ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_BASE_H + +#include "state/error_code.h" + +#include "costmap/costmap_interface.h" + +namespace roborts_global_planner{ + +class GlobalPlannerBase { + public: + typedef std::shared_ptr CostmapPtr; + + GlobalPlannerBase(CostmapPtr costmap_ptr) + : costmap_ptr_(costmap_ptr) { + }; + virtual ~GlobalPlannerBase() = default; + + virtual roborts_common::ErrorInfo Plan(const geometry_msgs::PoseStamped &start, + const geometry_msgs::PoseStamped &goal, + std::vector &path) = 0; + + protected: + CostmapPtr costmap_ptr_; +}; + +} //namespace roborts_global_planner + + +#endif // ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_BASE_H diff --git a/roborts_planning/global_planner/global_planner_node.cpp b/roborts_planning/global_planner/global_planner_node.cpp new file mode 100755 index 00000000..f40d9e3c --- /dev/null +++ b/roborts_planning/global_planner/global_planner_node.cpp @@ -0,0 +1,347 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include + +#include "global_planner_node.h" + +namespace roborts_global_planner{ + +using roborts_common::ErrorCode; +using roborts_common::ErrorInfo; +using roborts_common::NodeState; +GlobalPlannerNode::GlobalPlannerNode() : + new_path_(false),pause_(false), node_state_(NodeState::IDLE), error_info_(ErrorCode::OK), + as_(nh_,"global_planner_node_action",boost::bind(&GlobalPlannerNode::GoalCallback,this,_1),false) { + + if (Init().IsOK()) { + ROS_INFO("Global planner initialization completed."); + StartPlanning(); + as_.start(); + } else { + ROS_ERROR("Initialization failed."); + SetNodeState(NodeState::FAILURE); + } + +} + +ErrorInfo GlobalPlannerNode::Init() { + + // Load proto planning configuration parameters + GlobalPlannerConfig global_planner_config; + std::string full_path = ros::package::getPath("roborts_planning") + "/global_planner/config/global_planner_config.prototxt"; + if (!roborts_common::ReadProtoFromTextFile(full_path.c_str(), + &global_planner_config)) { + ROS_ERROR("Cannot load global planner protobuf configuration file."); + return ErrorInfo(ErrorCode::GP_INITILIZATION_ERROR, + "Cannot load global planner protobuf configuration file."); + } + + + selected_algorithm_ = global_planner_config.selected_algorithm(); + cycle_duration_ = std::chrono::microseconds((int) (1e6 / global_planner_config.frequency())); + max_retries_ = global_planner_config.max_retries(); + goal_distance_tolerance_ = global_planner_config.goal_distance_tolerance(); + goal_angle_tolerance_ = global_planner_config.goal_angle_tolerance(); + + // ROS path visualize + ros::NodeHandle viz_nh("~"); + path_pub_ = viz_nh.advertise("path", 10); + + // Create tf listener + tf_ptr_ = std::make_shared(ros::Duration(10)); + + // Create global costmap + std::string map_path = ros::package::getPath("roborts_costmap") + \ + "/config/costmap_parameter_config_for_global_plan.prototxt"; + costmap_ptr_ = std::make_shared("global_costmap", + *tf_ptr_, + map_path.c_str()); + // Create the instance of the selected algorithm + global_planner_ptr_ = roborts_common::AlgorithmFactory::CreateAlgorithm( + selected_algorithm_, costmap_ptr_); + if (global_planner_ptr_== nullptr) { + ROS_ERROR("global planner algorithm instance can't be loaded"); + return ErrorInfo(ErrorCode::GP_INITILIZATION_ERROR, + "global planner algorithm instance can't be loaded"); + } + + + // Initialize path frame from global costmap + path_.header.frame_id = costmap_ptr_->GetGlobalFrameID(); + return ErrorInfo(ErrorCode::OK); +} + +void GlobalPlannerNode::GoalCallback(const roborts_msgs::GlobalPlannerGoal::ConstPtr &msg) { + + ROS_INFO("Received a Goal from client!"); + + //Update current error and info + ErrorInfo error_info = GetErrorInfo(); + NodeState node_state = GetNodeState(); + + //Set the current goal + SetGoal(msg->goal); + + //If the last state is not running, set it to running + if (GetNodeState() != NodeState::RUNNING) { + SetNodeState(NodeState::RUNNING); + } + + //Notify the condition variable to stop lock waiting the fixed duration + { + std::unique_lock plan_lock(plan_mutex_); + plan_condition_.notify_one(); + } + + + + while (ros::ok()) { + // Preempted and Canceled + if (as_.isPreemptRequested()) { + if (as_.isNewGoalAvailable()) { + as_.setPreempted(); + ROS_INFO("Override!"); + break; + }else{ + as_.setPreempted(); + SetNodeState(NodeState::IDLE); + ROS_INFO("Cancel!"); + break; + } + } + + // Update the current state and error info + node_state = GetNodeState(); + error_info = GetErrorInfo(); + //TODO: seem useless to check state here, as it would never be IDLE state + if(node_state == NodeState::RUNNING || node_state == NodeState::SUCCESS || node_state == NodeState::FAILURE) { + roborts_msgs::GlobalPlannerFeedback feedback; + roborts_msgs::GlobalPlannerResult result; + // If error occurs or planner produce new path, publish the feedback + if (!error_info.IsOK() || new_path_) { + if (!error_info.IsOK()) { + feedback.error_code = error_info.error_code(); + feedback.error_msg = error_info.error_msg(); + SetErrorInfo(ErrorInfo::OK()); + } + if (new_path_) { + feedback.path = path_; + new_path_ = false; + } + as_.publishFeedback(feedback); + } + + // After get the result, deal with actionlib server and jump out of the loop + if(node_state == NodeState::SUCCESS){ + result.error_code = error_info.error_code(); + as_.setSucceeded(result,error_info.error_msg()); + SetNodeState(NodeState::IDLE); + break; + } + else if(node_state == NodeState::FAILURE){ + result.error_code = error_info.error_code(); + as_.setAborted(result,error_info.error_msg()); + SetNodeState(NodeState::IDLE); + break; + } + } + std::this_thread::sleep_for(std::chrono::microseconds(1)); + } + +} + +NodeState GlobalPlannerNode::GetNodeState() { + std::lock_guard node_state_lock(node_state_mtx_); + return node_state_; +} + +void GlobalPlannerNode::SetNodeState(NodeState node_state) { + std::lock_guard node_state_lock(node_state_mtx_); + node_state_ = node_state; +} + +ErrorInfo GlobalPlannerNode::GetErrorInfo() { + std::lock_guard error_info_lock(error_info_mtx_); + return error_info_; +} + +void GlobalPlannerNode::SetErrorInfo(ErrorInfo error_info) { + std::lock_guard node_state_lock(error_info_mtx_); + error_info_ = error_info; +} + +geometry_msgs::PoseStamped GlobalPlannerNode::GetGoal() { + std::lock_guard goal_lock(goal_mtx_); + return goal_; +} + +void GlobalPlannerNode::SetGoal(geometry_msgs::PoseStamped goal) { + std::lock_guard goal_lock(goal_mtx_); + goal_ = goal; +} + +void GlobalPlannerNode::StartPlanning() { + SetNodeState(NodeState::IDLE); + plan_thread_ = std::thread(&GlobalPlannerNode::PlanThread, this); +} + +void GlobalPlannerNode::StopPlanning() { + SetNodeState(NodeState::RUNNING); + if (plan_thread_.joinable()) { + plan_thread_.join(); + } +} + +void GlobalPlannerNode::PlanThread() { + ROS_INFO("Plan thread start!"); + geometry_msgs::PoseStamped current_start; + geometry_msgs::PoseStamped current_goal; + std::vector current_path; + std::chrono::microseconds sleep_time = std::chrono::microseconds(0); + ErrorInfo error_info; + int retries = 0; + while (ros::ok()) { + ROS_INFO("Wait to plan!"); + std::unique_lock plan_lock(plan_mutex_); + plan_condition_.wait_for(plan_lock, sleep_time); + while (GetNodeState()!=NodeState::RUNNING){ + std::this_thread::sleep_for(std::chrono::microseconds(1)); + } + ROS_INFO("Go on planning!"); + + std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); + + { + std::unique_lock lock(*(costmap_ptr_->GetCostMap()->GetMutex())); + bool error_set = false; + //Get the robot current pose + while (!costmap_ptr_->GetRobotPose(current_start)) { + if (!error_set) { + ROS_ERROR("Get Robot Pose Error."); + SetErrorInfo(ErrorInfo(ErrorCode::GP_GET_POSE_ERROR, "Get Robot Pose Error.")); + error_set = true; + } + std::this_thread::sleep_for(std::chrono::microseconds(1)); + } + + //Get the robot current goal and transform to the global frame + current_goal = GetGoal(); + + if (current_goal.header.frame_id != costmap_ptr_->GetGlobalFrameID()) { + current_goal = costmap_ptr_->Pose2GlobalFrame(current_goal); + SetGoal(current_goal); + } + + //Plan + error_info = global_planner_ptr_->Plan(current_start, current_goal, current_path); + + } + + if (error_info.IsOK()) { + //When planner succeed, reset the retry times + retries = 0; + PathVisualization(current_path); + + //Set the goal to avoid the same goal from getting transformed every time + current_goal = current_path.back(); + SetGoal(current_goal); + + //Decide whether robot reaches the goal according to tolerance + if (GetDistance(current_start, current_goal) < goal_distance_tolerance_ + && GetAngle(current_start, current_goal) < goal_angle_tolerance_ + ) { + SetNodeState(NodeState::SUCCESS); + } + } else if (max_retries_ > 0 && retries > max_retries_) { + //When plan failed to max retries, return failure + ROS_ERROR("Can not get plan with max retries( %d )", max_retries_ ); + error_info = ErrorInfo(ErrorCode::GP_MAX_RETRIES_FAILURE, "Over max retries."); + SetNodeState(NodeState::FAILURE); + retries=0; + } else if (error_info == ErrorInfo(ErrorCode::GP_GOAL_INVALID_ERROR)){ + //When goal is not reachable, return failure immediately + ROS_ERROR("Current goal is not valid!"); + SetNodeState(NodeState::FAILURE); + retries=0; + } + else { + //Increase retries + retries++; + ROS_ERROR("Can not get plan for once. %s", error_info.error_msg().c_str()); + } + // Set and update the error info + SetErrorInfo(error_info); + + // Deal with the duration to wait + std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); + std::chrono::microseconds execution_duration = + std::chrono::duration_cast(end_time - start_time); + sleep_time = cycle_duration_ - execution_duration; + + // Report warning while planning timeout + if (sleep_time <= std::chrono::microseconds(0)) { + ROS_ERROR("The time planning once is %ld beyond the expected time %ld", + execution_duration.count(), + cycle_duration_.count()); + sleep_time = std::chrono::microseconds(0); + SetErrorInfo(ErrorInfo(ErrorCode::GP_TIME_OUT_ERROR, "Planning once time out.")); + } + } + + + ROS_INFO("Plan thread terminated!"); +} + +void GlobalPlannerNode::PathVisualization(const std::vector &path) { + path_.poses = path; + path_pub_.publish(path_); + new_path_ = true; +} + +double GlobalPlannerNode::GetDistance(const geometry_msgs::PoseStamped &pose1, + const geometry_msgs::PoseStamped &pose2) { + const geometry_msgs::Point point1 = pose1.pose.position; + const geometry_msgs::Point point2 = pose2.pose.position; + const double dx = point1.x - point2.x; + const double dy = point1.y - point2.y; + return std::sqrt(dx * dx + dy * dy); +} + +double GlobalPlannerNode::GetAngle(const geometry_msgs::PoseStamped &pose1, + const geometry_msgs::PoseStamped &pose2) { + const geometry_msgs::Quaternion quaternion1 = pose1.pose.orientation; + const geometry_msgs::Quaternion quaternion2 = pose2.pose.orientation; + tf::Quaternion rot1, rot2; + tf::quaternionMsgToTF(quaternion1, rot1); + tf::quaternionMsgToTF(quaternion2, rot2); + return rot1.angleShortestPath(rot2); +} + +GlobalPlannerNode::~GlobalPlannerNode() { + StopPlanning(); +} + +} //namespace roborts_global_planner + +int main(int argc, char **argv) { + + ros::init(argc, argv, "global_planner_node"); + roborts_global_planner::GlobalPlannerNode global_planner; + ros::spin(); + return 0; +} diff --git a/roborts_planning/global_planner/global_planner_node.h b/roborts_planning/global_planner/global_planner_node.h new file mode 100755 index 00000000..025729d5 --- /dev/null +++ b/roborts_planning/global_planner/global_planner_node.h @@ -0,0 +1,196 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H +#define ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H + + +#include +#include +#include +#include + +#include +#include "actionlib/server/simple_action_server.h" +#include "nav_msgs/Path.h" +#include "roborts_msgs/GlobalPlannerAction.h" + +#include "alg_factory/algorithm_factory.h" +#include "state/error_code.h" +#include "io/io.h" +#include "state/node_state.h" + +#include "costmap/costmap_interface.h" +#include "global_planner_base.h" +#include "proto/global_planner_config.pb.h" +#include "global_planner_algorithms.h" + +namespace roborts_global_planner{ + +/** + * @brief Node class for global planner module. + */ +class GlobalPlannerNode { + public: + typedef std::shared_ptr CostmapPtr; + typedef std::shared_ptr TfPtr; + typedef std::unique_ptr GlobalPlannerPtr; + typedef actionlib::SimpleActionServer GlobalPlannerServer; + /** + * @brief Constructor including all initialization and configuration + */ + GlobalPlannerNode(); + + /** + * @brief Destructor which stops all running threads . + */ + ~GlobalPlannerNode(); + + private: + /** + * @brief Initiate the ROS configuration, planning algorithm instance, + * planning related parameter configuration and etc. + * @return ErrorInfo which is OK if succeed + */ + roborts_common::ErrorInfo Init(); + /** + * @brief Callback function for the actionlib task + * @param msg The task goal message from actionlib client + */ + void GoalCallback(const roborts_msgs::GlobalPlannerGoal::ConstPtr &msg); + /** + * @brief Set the state for planning module + * @param node_state Enumerate State for global planning + */ + void SetNodeState(roborts_common::NodeState node_state); + /** + * @brief Get the state for planning module + * @return Enumerate State for global planning + */ + roborts_common::NodeState GetNodeState(); + /** + * @brief Set the state for planning module + * @param error_info Error Info for global planning + */ + void SetErrorInfo(roborts_common::ErrorInfo error_info); + /** + * @brief Get the error info for planning module + * @return Error Info for global planning + */ + roborts_common::ErrorInfo GetErrorInfo(); + /** + * @brief Check if the plan thread is still under execution, if not, start the plan thread. + * @return True if start the plan thread successful, else false. + */ + geometry_msgs::PoseStamped GetGoal(); + /** + * @brief Set the planner goal + * @param goal planner goal + */ + void SetGoal(geometry_msgs::PoseStamped goal); + /** + * @brief Start the plan thread and set the state to RUNNING + */ + void StartPlanning(); + /** + * @brief Stop the plan thread and set the state to IDLE + */ + void StopPlanning(); + /** + * @brief Plan thread which mainly include a planning cycle process: + * 1. Get the current start pose and goal pose, validate and transform them according to frame of costmap \n + * 2. Make a plan using the selected algorithm \n + * 3. If success, check the completion if the current pose is close to the goal. If completed, no need of planning and jump out of the cycle!\n + * 4. If not succeed, check if it reaches the max retries. if so, no need of planning and jump out of the cycle!\n + * 5. If planning still needed, go to next cycle and wait for a duration which the global planning frequency decides. + */ + void PlanThread(); + /** + * @brief Get the Euclidean distance of two poses. + * @param pose1 The first pose in the form of geometry_msgs::PoseStamped + * @param pose2 The second pose in the form of geometry_msgs::PoseStamped + * @return The Euclidean distance + */ + double GetDistance(const geometry_msgs::PoseStamped& pose1, + const geometry_msgs::PoseStamped& pose2); + /** + * @brief Get the angle difference of two pose . + * @param pose1 The first pose in the form of geometry_msgs::PoseStamped + * @param pose2 The second pose in the form of geometry_msgs::PoseStamped + * @return The angle difference + */ + double GetAngle(const geometry_msgs::PoseStamped &pose1, + const geometry_msgs::PoseStamped &pose2); + /** + * @brief Input the path in geometry_msgs::PoseStamped vector and publish the path in nav_msgs::Path + * @param path the path generated from global planner, indeed the discrete poses in the form of geometry_msgs::PoseStamped vector + */ + void PathVisualization(const std::vector &path); + + //! ROS Node Handler + ros::NodeHandle nh_; + //! ROS Publisher for path visualization in rviz + ros::Publisher path_pub_; + //! ROS Actionlib Server for command global planning module + GlobalPlannerServer as_; + //! Global planner pointer + GlobalPlannerPtr global_planner_ptr_; + //！ Transform pointer + TfPtr tf_ptr_; + //! Costmap pointer + CostmapPtr costmap_ptr_; + //! String Type for selected algorithm + std::string selected_algorithm_; + + //! Received goal for global planner (Input) + geometry_msgs::PoseStamped goal_; + //! Goal mutex + std::mutex goal_mtx_; + //! Path generated by global planner (Output) + bool pause_; + + nav_msgs::Path path_; + //! Bool flag that indicates whether or not new plan path comes + bool new_path_; + + //! Thread for global planning progress + std::thread plan_thread_; + //! Planning condition variable + std::condition_variable plan_condition_; + //! Planning mutex + std::mutex plan_mutex_; + + //! Global planner node state + roborts_common::NodeState node_state_; + //! Global planner node state mutex + std::mutex node_state_mtx_; + //! Global planner error infomation + roborts_common::ErrorInfo error_info_; + //! Global planner error infomation mutex + std::mutex error_info_mtx_; + + //! Cycle Duration in microseconds + std::chrono::microseconds cycle_duration_; + //! Max retry count + int max_retries_; + //! Distance tolerance towards goal + double goal_distance_tolerance_; + //! Angle tolerance towards goal + double goal_angle_tolerance_; +}; + +} //namespace roborts_global_planner +#endif //ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H diff --git a/roborts_planning/global_planner/global_planner_test.cpp b/roborts_planning/global_planner/global_planner_test.cpp new file mode 100755 index 00000000..94fe2079 --- /dev/null +++ b/roborts_planning/global_planner/global_planner_test.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include + +#include +#include +#include + +#include "roborts_msgs/GlobalPlannerAction.h" +#include "state/error_code.h" + +using roborts_common::ErrorCode; + +class GlobalPlannerTest{ + public: + GlobalPlannerTest(): + global_planner_actionlib_client_("global_planner_node_action", true){ + ros::NodeHandle rviz_nh("move_base_simple"); + goal_sub_ = rviz_nh.subscribe("goal", 1, + &GlobalPlannerTest::GoalCallback,this); + + global_planner_actionlib_client_.waitForServer(); + } + ~GlobalPlannerTest() = default; + + void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr & goal){ + ROS_INFO("Get new goal."); + command_.goal = *goal; + global_planner_actionlib_client_.sendGoal(command_, + boost::bind(&GlobalPlannerTest::DoneCallback, this, _1, _2), + boost::bind(&GlobalPlannerTest::ActiveCallback, this), + boost::bind(&GlobalPlannerTest::FeedbackCallback, this, _1) + ); + } + + void DoneCallback(const actionlib::SimpleClientGoalState& state, const roborts_msgs::GlobalPlannerResultConstPtr& result){ + ROS_INFO("The goal is done with %s!",state.toString().c_str()); + } + void ActiveCallback() { + ROS_INFO("Action server has recived the goal, the goal is active!"); + } + void FeedbackCallback(const roborts_msgs::GlobalPlannerFeedbackConstPtr& feedback){ + if (feedback->error_code != ErrorCode::OK) { + ROS_INFO("%s", feedback->error_msg.c_str()); + } + if (!feedback->path.poses.empty()) { + ROS_INFO("Get Path!"); + } + } + private: + ros::Subscriber goal_sub_; + roborts_msgs::GlobalPlannerGoal command_; + actionlib::SimpleActionClient global_planner_actionlib_client_; +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "global_planner_test"); + GlobalPlannerTest global_planner_test; + ros::spin(); + return 0; +} + diff --git a/roborts_planning/global_planner/proto/global_planner_config.proto b/roborts_planning/global_planner/proto/global_planner_config.proto new file mode 100755 index 00000000..6afdc028 --- /dev/null +++ b/roborts_planning/global_planner/proto/global_planner_config.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package roborts_global_planner; + +message GlobalPlannerConfig { + repeated string name = 1; + optional string selected_algorithm = 2; + required int32 frequency = 3; + required int32 max_retries = 4; + required double goal_distance_tolerance = 5; + required double goal_angle_tolerance = 6; +} diff --git a/roborts_planning/local_planner/CMakeLists.txt b/roborts_planning/local_planner/CMakeLists.txt new file mode 100644 index 00000000..f7fbcee4 --- /dev/null +++ b/roborts_planning/local_planner/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.0) +project(local_planner) + + +set(EXTERNAL_INCLUDE_DIRS + ${EIGEN3_INCLUDE_DIR} + ${SUITESPARSE_INCLUDE_DIRS} + ${G2O_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} + ) +set(EXTERNAL_LIBS + ${SUITESPARSE_LIBRARIES} + ${G2O_LIBRARIES} + ${catkin_LIBRARIES} + ${PROTOBUF_LIBRARIES} + roborts_costmap + ) + +include_directories(${EXTERNAL_INCLUDE_DIRS} include/ timed_elastic_band/include/) + +add_subdirectory(timed_elastic_band) + +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto/*.proto") +rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto + LocalPlanProtoSrc + LocalPlanProtoHds + ${ProtoFiles} + ) + +aux_source_directory(src/. SRC_LIST) + +#local planner node +add_executable(${PROJECT_NAME}_node + ${LocalPlanProtoSrc} + ${LocalPlanProtoHds} + src/local_planner_node.cpp + src/local_visualization.cpp + src/obstacles.cpp + src/odom_info.cpp + src/robot_position_cost.cpp + ) + +target_link_libraries(${PROJECT_NAME}_node + PRIVATE + planning::timed_elastic_band + ${EXTERNAL_LIBS} + ) +add_dependencies(${PROJECT_NAME}_node + roborts_msgs_generate_messages) + +#local palnner test +add_executable(teb_test + ${LocalPlanProtoSrc} + ${LocalPlanProtoHds} + src/teb_test.cpp + src/local_visualization.cpp + src/obstacles.cpp + src/odom_info.cpp + src/robot_position_cost.cpp + ) + +target_link_libraries(teb_test + PRIVATE + planning::timed_elastic_band + ${EXTERNAL_LIBS} + ) +add_dependencies(teb_test + roborts_msgs_generate_messages) + +#vel converter +add_executable(vel_converter + src/vel_converter.cpp + ) + +target_link_libraries(vel_converter + PRIVATE + ${EXTERNAL_LIBS} + ) +add_dependencies(vel_converter + roborts_msgs_generate_messages) diff --git a/roborts_planning/local_planner/README.md b/roborts_planning/local_planner/README.md new file mode 100644 index 00000000..e69de29b diff --git a/roborts_planning/local_planner/config/local_planner.prototxt b/roborts_planning/local_planner/config/local_planner.prototxt new file mode 100644 index 00000000..81bb6da1 --- /dev/null +++ b/roborts_planning/local_planner/config/local_planner.prototxt @@ -0,0 +1,4 @@ +name: "timed_elastic_band" +name: "DWA" +selected_algorithm: "timed_elastic_band" +frequency: 40 diff --git a/roborts_planning/local_planner/include/local_planner/data_base.h b/roborts_planning/local_planner/include/local_planner/data_base.h new file mode 100644 index 00000000..d6590841 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/data_base.h @@ -0,0 +1,277 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_DATA_CONVERT_BASE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_DATA_CONVERT_BASE_H + +#include + +#include +#include +#include +#include + +namespace roborts_local_planner { +/** + * @brief Class of SE(2) data base which is used by teb_optimal + */ +class DataBase { + public: + /** + * @brief default Constructor all elements will be set to zero + */ + DataBase () { + SetZero(); + } + + /** + * @brief Constructor with param + * @param position Position in optimal frame + * @param theta Orientation in optimal frame + */ + DataBase (const Eigen::Ref& position, double theta) { + position_ = position; + theta_ = theta; + } + + /** + * @brief Destructor + */ + ~DataBase() {} + + /** + * @brief Set position param + * @param position The new value of position_ + */ + void SetPosition(Eigen::Vector2d position) { + position_ = position; + } + + /** + * @brief Set orientation + * @param theta The new value of theta_ + */ + void SetTheta(double theta) { + theta_ = theta; + } + + /** + * @brief Get positon_ + * @return The reference of position_ + */ + Eigen::Vector2d& GetPosition () { + return position_; + } + + /** + * @brief Get positon_ + * @return The reference of position_ + */ + const Eigen::Vector2d& GetPosition () const { + return position_; + } + + /** + * @brief Get orientation + * @return The reference of theta_ + */ + double& GetTheta() { + return theta_; + } + + /** + * @brief Get orientation + * @return The reference of theta_ + */ + const double& GetTheta() const { + return theta_; + } + + /** + * @brief Add operation with array + * @param pose_array a array include x, y, theta. + */ + void Plus (const double* pose_array) { + position_.coeffRef(0) += pose_array[0]; + position_.coeffRef(1) += pose_array[1]; + theta_ = g2o::normalize_theta( theta_ + pose_array[2] ); + } + + /** + * @brief Average operation in two pose. + * @param pose1 The first pose + * @param pose2 The second pose + */ + void AverageInPlace (const DataBase& pose1, const DataBase& pose2) { + position_ = (pose1.position_ + pose2.position_)/2; + theta_ = g2o::average_angle(pose1.theta_, pose2.theta_); + } + + /** + * @brief Rotate operation + * @param angle Rotate angle + * @param adjust_theta If true will normalize the theta_ with angle, otherewise will keep the orientation in the new position + */ + void RotateGlobal (double angle, bool adjust_theta=true) { + double new_x = std::cos(angle)*position_.x() - std::sin(angle)*position_.y(); + double new_y = std::sin(angle)*position_.x() + std::cos(angle)*position_.y(); + position_.x() = new_x; + position_.y() = new_y; + if (adjust_theta) + theta_ = g2o::normalize_theta(theta_+angle); + } + + /** + * @brief Obtain the orientation vector + * @return A orientation vector in form of Eigen::Vector2d + */ + Eigen::Vector2d OrientationUnitVec() const { + return Eigen::Vector2d(std::cos(theta_), std::sin(theta_)); + } + + /** + * @brief Set all elements to zero + */ + void SetZero() { + position_.setZero(); + theta_ = 0; + } + + /** + * @brief Overload '=' operator + * @param rhs The right value + * @return A reference of the new value + */ + DataBase& operator= ( const DataBase& rhs ) { + if (&rhs != this) { + position_ = rhs.position_; + theta_ = rhs.theta_; + } + return *this; + } + + /** + * @brief Overload '+=' operator + * @param rhs The right value + * @return A reference of the new value + */ + DataBase& operator+= (const DataBase& rhs) { + position_ += rhs.position_; + theta_ = g2o::normalize_theta(theta_ + rhs.theta_); + return *this; + } + + /** + * @brief Overload '+' operator + * @param lhs The left value + * @param rhs The right value + * @return New value after '+' operation + */ + friend DataBase operator+ (DataBase lhs, const DataBase& rhs) { + return lhs += rhs; + } + + /** + * @brief Overload '-=' operator + * @param rhs The right value + * @return A reference of new value + */ + DataBase& operator-= (const DataBase& rhs) { + theta_ -= rhs.theta_; + theta_ = g2o::normalize_theta(theta_ - rhs.theta_); + return *this; + } + + /** + * @brief Overload '-' operator + * @param lhs The left value + * @param rhs The right value + * @return New value after '-' operation + */ + friend DataBase operator- (DataBase lhs, const DataBase& rhs) { + return lhs -= rhs; + } + + /** + * @brief Overload '*' operator + * @param pose Data want to scale + * @param scalar Scaling factor + * @return Data after scale + */ + friend DataBase operator* (DataBase pose, double scalar) { + pose.position_ *= scalar; + pose.theta_ *= scalar; + return pose; + } + + + + private: + //! x, y data + Eigen::Vector2d position_; + //! orientation + double theta_; + + public: + //! Data alignment + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; // class TebDataBase + + +} // namespace roborts_local_planner + + +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_DATA_CONVERT_BASE_H \ No newline at end of file diff --git a/roborts_planning/local_planner/include/local_planner/data_converter.h b/roborts_planning/local_planner/include/local_planner/data_converter.h new file mode 100644 index 00000000..e93b52ae --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/data_converter.h @@ -0,0 +1,117 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_DATA_CONVERTER_H_ +#define ROBORTS_PLANNING_LOCAL_PLANNER_DATA_CONVERTER_H_ + +#include +#include +#include + +#include "costmap/costmap_interface.h" + +/** + * @brief Convert Data to Eigen, all function is static + * @usage DataConverter::FUNCTION(param, param, ...) + * @return std::pair or Eigen::Vector2d + */ +namespace roborts_local_planner { +class DataConverter { + public: + /** + * @brief Convert roborts_costmap::RobotPose to std::pair + * @param pose Data structure of roborts_costmap::RobotPose + * @return Data structure of std::pair + */ + static std::pair LocalConvertRMData(const roborts_costmap::RobotPose &pose) { + Eigen::Vector2d position; + position.coeffRef(0) = pose.position.coeffRef(0); + position.coeffRef(1) = pose.position.coeffRef(1); + Eigen::Vector3f euler = pose.rotation.eulerAngles(2, 1, 0); + return std::make_pair(position, euler(0, 0)); + } + + /** + * @brief Convert tf::Pose to std::pair + * @param pose Type of tf::Pose + * @return Type of std::pair + */ + static std::pair LocalConvertTFData(const tf::Pose &pose) { + Eigen::Vector2d position; + position.coeffRef(0) = pose.getOrigin().getX(); + position.coeffRef(1) = pose.getOrigin().getY(); + return std::make_pair(position, tf::getYaw(pose.getRotation())); + } + + /** + * @brief Convert geometry_msgs::Pose to std::pair + * @param pose Type of geometry_msgs::Pose + * @return Type of std::pair + */ + static std::pair LocalConvertGData(const geometry_msgs::Pose &pose) { + Eigen::Vector2d position; + position.coeffRef(0) = pose.position.x; + position.coeffRef(1) = pose.position.y; + return std::make_pair(position, tf::getYaw(pose.orientation)); + } + + /** + * @brief Convert geometry_msgs::Point to Eigen::Vector2d + * @param point Type of geometry_msgs::Point + * @return Type of Eigen::Vector2d + */ + static Eigen::Vector2d LocalConvertGData(const geometry_msgs::Point &point) { + Eigen::Vector2d position; + position.coeffRef(0) = point.x; + position.coeffRef(1) = point.y; + return position; + } + + /** + * @brief Convert std::vector to Eigen::Vector2d + * @param points vector of geometry_msgs::Point + * @return vector of Eigen::Vector2d + */ + static std::vector LocalConvertGData(const std::vector &points) { + std::vector positions; + for (int i = 0; i < points.size(); ++i) { + Eigen::Vector2d temp_point; + temp_point = LocalConvertGData (points[i]); + positions.push_back(temp_point); + } + return positions; + } + + /** + * Convert common type to Eigen::Vector2d + * @param x Position x + * @param y Position y + * @param theta Orientation theta + * @return std::pair + */ + static std::pair LocalConvertCData(double x, double y, double theta) { + Eigen::Vector2d position; + position.coeffRef(0) = x; + position.coeffRef(1) = y; + return std::make_pair(position, theta); + } +}; + + +} // namespace roborts_local_planner +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_DATA_CONVERTER_H_ + diff --git a/roborts_planning/local_planner/include/local_planner/distance_calculation.h b/roborts_planning/local_planner/include/local_planner/distance_calculation.h new file mode 100644 index 00000000..25957532 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/distance_calculation.h @@ -0,0 +1,289 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_DISTANCE_CALCULATIONS_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_DISTANCE_CALCULATIONS_H + +#include +#include "local_planner/utility_tool.h" + +namespace roborts_local_planner { + +/** + * @brief This file is a distance calculator + */ + +//! A point container +typedef std::vector > Point2dContainer; + +/** + * @brief Calculate a closest point from a point to 2D line segment + * @param point Input point + * @param line_start Start point of line + * @param line_end End Point of line + * @return Closest point on line + */ +inline Eigen::Vector2d ClosestPointOnLineSegment2D(const Eigen::Ref &point, + const Eigen::Ref &line_start, + const Eigen::Ref &line_end) { + Eigen::Vector2d diff = line_end - line_start; + double sq_norm = diff.squaredNorm(); + + if (sq_norm == 0){ + return line_start; + } + + double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y()) * diff.y()) / sq_norm; + + if (u <= 0) { + return line_start; + } else if (u >= 1) { + return line_end; + } + + return line_start + u * diff; +} + +/** + * @brief Calculate distance from point to 2D line segment + * @param point Input point + * @param line_start Start point of line + * @param line_end End Point of line + * @return Distance + */ + +inline double DistancePointToSegment2D(const Eigen::Ref &point, + const Eigen::Ref &line_start, + const Eigen::Ref &line_end) { + return (point - ClosestPointOnLineSegment2D(point, line_start, line_end)).norm(); +} + +/** + * @brief Calculate if two 2D line segment intersection + * @param line1_start First line start point + * @param line1_end Firts line end point + * @param line2_start Second line start point + * @param line2_end Second line end point + * @param intersection Point where intersection + * @return If true intersection, else not intersection + */ +inline bool CheckLineSegmentsIntersection2D(const Eigen::Ref &line1_start, + const Eigen::Ref &line1_end, + const Eigen::Ref &line2_start, + const Eigen::Ref &line2_end, + Eigen::Vector2d *intersection = NULL) { + double s_numer, t_numer, denom, t; + Eigen::Vector2d line1 = line1_end - line1_start; + Eigen::Vector2d line2 = line2_end - line2_start; + + denom = line1.x() * line2.y() - line2.x() * line1.y(); + if (denom == 0) { + return false; + } + bool denomPositive = denom > 0; + + Eigen::Vector2d aux = line1_start - line2_start; + + s_numer = line1.x() * aux.y() - line1.y() * aux.x(); + if ((s_numer < 0) == denomPositive) { + return false; + } + + t_numer = line2.x() * aux.y() - line2.y() * aux.x(); + if ((t_numer < 0) == denomPositive) { + return false; + } + + if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) { + return false; + } + + t = t_numer / denom; + if (intersection) { + *intersection = line1_start + t * line1; + } + + return true; +} + +/** + * @brief Calculate two 2D segment lines' distance + * @param line1_start First line start point + * @param line1_end Firts line end point + * @param line2_start Second line start point + * @param line2_end Second line end point + * @return distance of two 2D segment lines + */ + +inline double DistanceSegmentToSegment2D(const Eigen::Ref &line1_start, + const Eigen::Ref &line1_end, + const Eigen::Ref &line2_start, + const Eigen::Ref &line2_end) { + + if (CheckLineSegmentsIntersection2D(line1_start, line1_end, line2_start, line2_end)){ + return 0; + } + + std::array distances; + + distances[0] = DistancePointToSegment2D(line1_start, line2_start, line2_end); + distances[1] = DistancePointToSegment2D(line1_end, line2_start, line2_end); + distances[2] = DistancePointToSegment2D(line2_start, line1_start, line1_end); + distances[3] = DistancePointToSegment2D(line2_end, line1_start, line1_end); + + return *std::min_element(distances.begin(), distances.end()); +} + +/** + * @brief Calculate point to 2D polygon distance + * @param point Input point + * @param vertices 2D polygon's vertices + * @return distance of point to 2D polygon + */ +inline double DistancePointToPolygon2D(const Eigen::Vector2d &point, const Point2dContainer &vertices) { + double dist = HUGE_VAL; + + if (vertices.size() == 1) { + return (point - vertices.front()).norm(); + } + + for (int i = 0; i < (int) vertices.size() - 1; ++i) { + + double new_dist = PointToLineDistance(point.coeffRef(0), point.coeffRef(1), vertices.at(i).coeffRef(0), vertices.at(i).coeffRef(1), + vertices.at(i + 1).coeffRef(0), vertices.at(i + 1).coeffRef(1)); + if (new_dist < dist) { + dist = new_dist; + } + + } + + /*if (vertices.size() > 2) { + double new_dist = PointToLineDistance(point.coeffRef(0), point.coeffRef(1), vertices.back().coeffRef(0), vertices.back().coeffRef(1), + vertices.front().coeffRef(0), vertices.front().coeffRef(1)); + if (new_dist < dist) { + std::cout << "here" << std::endl; + dist = new_dist; + } + }*/ + + return dist; +} + +/** + * @brief Calculate 2D line segment to 2D polygon distance + * @param line_start Line start point + * @param line_end Line end point + * @param vertices Vertices 2D polygon's vertices + * @return distance of 2D line segment to 2D polygon + */ +inline double DistanceSegmentToPolygon2D(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + const Point2dContainer &vertices) { + double dist = HUGE_VAL; + + if (vertices.size() == 1) { + return DistancePointToSegment2D(vertices.front(), line_start, line_end); + } + + for (int i = 0; i < (int) vertices.size() - 1; ++i) { + double new_dist = DistanceSegmentToSegment2D(line_start, line_end, vertices.at(i), vertices.at(i + 1)); + if (new_dist < dist) { + dist = new_dist; + } + } + + if (vertices.size() > 2) { + double new_dist = + DistanceSegmentToSegment2D(line_start, line_end, vertices.back(), vertices.front()); + if (new_dist < dist) + return new_dist; + } + + return dist; +} + +/** + * @brief Calculate two 2D polygons' distance + * @param vertices1 Vertices of first 2D polygon's vertices + * @param vertices2 Vertices of second 2D polygon's vertices + * @return distance of two 2D polygons + */ +inline double DistancePolygonToPolygon2D(const Point2dContainer &vertices1, const Point2dContainer &vertices2) { + double dist = HUGE_VAL; + + if (vertices1.size() == 1) { + return DistancePointToPolygon2D(vertices1.front(), vertices2); + } + + for (int i = 0; i < (int) vertices1.size() - 1; ++i) { + double new_dist = DistanceSegmentToPolygon2D(vertices1[i], vertices1[i + 1], vertices2); + if (new_dist < dist) { + dist = new_dist; + } + } + + if (vertices1.size() > 2) { + double new_dist = DistanceSegmentToPolygon2D(vertices1.back(), vertices1.front(), vertices2); + if (new_dist < dist) { + return new_dist; + } + } + + return dist; +} + + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_DISTANCE_CALCULATIONS_H diff --git a/roborts_planning/local_planner/include/local_planner/line_iterator.h b/roborts_planning/local_planner/include/local_planner/line_iterator.h new file mode 100644 index 00000000..541d63a3 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/line_iterator.h @@ -0,0 +1,168 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_LINE_ITERATOR_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_LINE_ITERATOR_H + +#include + +namespace roborts_local_planner { + +/** + * @brief This class do iterator operation in a grid map + */ +class FastLineIterator { + // this method is a modified version of base_local_planner/line_iterator.h + public: + + /** + * @brief Class Constructor + * @param x0 Line start x + * @param y0 Line start y + * @param x1 Line end x + * @param y1 Line end y + */ + FastLineIterator( int x0, int y0, int x1, int y1 ) + : x0_( x0 ) + , y0_( y0 ) + , x1_( x1 ) + , y1_( y1 ) + , x_( x0 ) + , y_( y0 ) + , deltax_(abs(x1 - x0)) + , deltay_(abs(y1 - y0)) + , curpixel_( 0 ) { + + xinc1_ = (x1 - x0) >0 ?1:-1; + xinc2_ = (x1 - x0) >0 ?1:-1; + + yinc1_ = (y1 - y0) >0 ?1:-1; + yinc2_ = (y1 - y0) >0 ?1:-1; + + if( deltax_ >= deltay_ ) { + xinc1_ = 0; + yinc2_ = 0; + den_ = 2 * deltax_; + num_ = deltax_; + numadd_ = 2 * deltay_; + numpixels_ = deltax_; + } else { + xinc2_ = 0; + yinc1_ = 0; + den_ = 2 * deltay_; + num_ = deltay_; + numadd_ = 2 * deltax_; + numpixels_ = deltay_; + } + } + + ~FastLineIterator() = default; + + /** + * @brief Line iterator's end condition + * @return If true continue iteration, else finish line iteration + */ + bool IsValid() const { + return curpixel_ <= numpixels_; + } + + /** + * @brief Do add iteration add + */ + void Advance() { + num_ += numadd_; + if( num_ >= den_ ) { + num_ -= den_; + x_ += xinc1_; + y_ += yinc1_; + } + x_ += xinc2_; + y_ += yinc2_; + + curpixel_++; + } + + /** + * @brief Get current x + * @return Current x + */ + int GetX() const { return x_; } + /** + * @brief Get current y + * @return Current y + */ + int GetY() const { return y_; } + + /** + * @brief Get start x + * @return Start x + */ + int GetX0() const { return x0_; } + /** + * @brief Get start y + * @return Start y + */ + int GetY0() const { return y0_; } + + /** + * @brief Get end x + * @return End x + */ + int GetX1() const { return x1_; } + /** + * @brief Get end y + * @return End y + */ + int GetY1() const { return y1_; } + + private: + //! x0_ Line start x + int x0_; + //! y0_ Line start y + int y0_; + //! x1_ Line end x + int x1_; + //! y1_ Line end y + int y1_; + + //! Iterator value + int x_; + int y_; + + //! total change + int deltax_; + int deltay_; + + //! current iterator steep + int curpixel_; + + //! xinc1_, yinc1_ x_ and y_ increase value if num_ > den_ + //! xinc2_, yinc2_ x_ and y_ increase value if num_ < den_ + int xinc1_, xinc2_, yinc1_, yinc2_; + + //! numpixels_ total iterator steep + //! num_ current error + //! numadd_ error should add after a iterator + //! den_ max error + int den_, num_, numadd_, numpixels_; +}; + + +} // namespace roborts_local_planner + +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_LINE_ITERATOR_H + diff --git a/roborts_planning/local_planner/include/local_planner/local_planner_algorithms.h b/roborts_planning/local_planner/include/local_planner/local_planner_algorithms.h new file mode 100644 index 00000000..b83211a2 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/local_planner_algorithms.h @@ -0,0 +1,30 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_ALGORITHMS_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_ALGORITHMS_H +/** + * @brief all local planner algorithm's header file should be include in this file + */ + +/** + * @brief timed elastic band algorithm's header file + */ +#include "timed_elastic_band/teb_local_planner.h" + +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_ALGORITHMS_H + diff --git a/roborts_planning/local_planner/include/local_planner/local_planner_base.h b/roborts_planning/local_planner/include/local_planner/local_planner_base.h new file mode 100644 index 00000000..d54ccc1f --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/local_planner_base.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ + +#ifndef ROBORTS_PLANNING_BASE_LOCAL_PLANNER_H +#define ROBORTS_PLANNING_BASE_LOCAL_PLANNER_H + +#include + +#include + +#include + +#include "state/error_code.h" +#include "costmap/costmap_interface.h" +#include "local_planner/local_visualization.h" +#include "roborts_msgs/TwistAccel.h" + +/** + * @brief parent class of all local planner algorithms + */ +namespace roborts_local_planner { + +//! Error info callback function +typedef boost::function ErrorInfoCallback; +class LocalPlannerBase { + public: + /** + * @brief Virtual function calculate robot velocity should be implemented by derived class + * @param cmd_vel Velocity use to control robot + * @return Error info + */ + virtual roborts_common::ErrorInfo ComputeVelocityCommands(roborts_msgs::TwistAccel &cmd_vel) = 0; + + /** + * @brief Virtual function judge if robot reach the goal should be implemented by derived class + * @return If true reached the goal, else not + */ + virtual bool IsGoalReached() = 0; + + /** + * @brief Virtual function initialize local planner algorithm should be implemented by derived class + * @param local_cost Local cost map + * @param tf Tf listener + * @param visual Visualize pointer + * @return Error info + */ + virtual roborts_common::ErrorInfo Initialize(std::shared_ptr local_cost, + std::shared_ptr tf, LocalVisualizationPtr visual) = 0; + + /** + * @brief Virtual function Set global plan's result to local planner or set a goal to local planner + * should be implement by derived class + * @param plan Result of global planner + * @param goal Goal of local planner + * @return If true success, else fail + */ + virtual bool SetPlan(const nav_msgs::Path& plan, const geometry_msgs::PoseStamped& goal) = 0; + + /** + * @brief Virtual function Register error callback function should be implemented by derived class + * @param error_callback Callback function + */ + virtual void RegisterErrorCallBack(ErrorInfoCallback error_callback) = 0; + virtual ~LocalPlannerBase(){} + + protected: + LocalPlannerBase () {} +}; + +typedef boost::shared_ptr LocalPlannerPtr; + +} // namespace roborts_local_planner + + + + +#endif //ROBORTS_PLANNING_BASE_LOCAL_PLANNER_H diff --git a/roborts_planning/local_planner/include/local_planner/local_planner_node.h b/roborts_planning/local_planner/include/local_planner/local_planner_node.h new file mode 100644 index 00000000..79643cb4 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/local_planner_node.h @@ -0,0 +1,171 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_NODE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_NODE_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "io/io.h" +#include "state/error_code.h" +#include "state/node_state.h" +#include "alg_factory/algorithm_factory.h" + +#include "costmap/costmap_interface.h" +#include "roborts_msgs/LocalPlannerAction.h" +#include "roborts_msgs/TwistAccel.h" + + +#include "local_planner/proto/local_planner.pb.h" +#include "local_planner/local_planner_base.h" +#include "local_planner/local_visualization.h" +#include "local_planner/local_planner_algorithms.h" + +namespace roborts_local_planner { + +/** + * @brief Local planner node class + */ +class LocalPlannerNode { + public: + /** + * @brief Constructor + */ + LocalPlannerNode(); + ~LocalPlannerNode(); + + /** + * @brief init all param + * @return Error info + */ + roborts_common::ErrorInfo Init(); + + /** + * @brief Main loop + */ + void Loop(); + + /** + * @brief Actionlib callback function use to control loop function + * @param command Command to control loop function + */ + void ExcuteCB(const roborts_msgs::LocalPlannerGoal::ConstPtr &command); + + /** + * @brief local planner algorithm's error callback function + * @param algorithm_error_info local planner algorithm's error info + */ + void AlgorithmCB(const roborts_common::ErrorInfo &algorithm_error_info); + + /** + * @brief start local planner algorithm + */ + void StartPlanning(); + + /** + * @brief stop local planner algorithm + */ + void StopPlanning(); + + /** + * @brief Set local planner node state + * @param node_state State want to set + */ + void SetNodeState(const roborts_common::NodeState& node_state); + + /** + * @brief Get local planner node state + * @return State of local planner node + */ + roborts_common::NodeState GetNodeState(); + + /** + * @brief Set Error info if error occur + * @param error_info error info + */ + void SetErrorInfo(const roborts_common::ErrorInfo error_info); + + /** + * Get error info + * @return Error in local planner node + */ + roborts_common::ErrorInfo GetErrorInfo(); + + private: + + //! ros node handle + ros::NodeHandle local_planner_nh_; + //! local planner algorithm thread + std::thread local_planner_thread_; + //! local planner node actionlib server + actionlib::SimpleActionServer as_; + //! local planner algorithm parent pointer + std::unique_ptr local_planner_; + //! node state mutex + std::mutex node_state_mtx_; + //! node error info mutex + std::mutex node_error_info_mtx_; + //! planner algorithm mutex + std::mutex plan_mtx_; + //! node state + roborts_common::NodeState node_state_; + //! error info + roborts_common::ErrorInfo node_error_info_; + //! local cost map + std::shared_ptr local_cost_; + //! tf pointer + std::shared_ptr tf_; + //! initialize state + bool initialized_; + + //! local planner algorithm which choose to run + std::string selected_algorithm_; + + //geometry_msgs::Twist cmd_vel_; + //! robot control velocity with accelerate + roborts_msgs::TwistAccel cmd_vel_; + //! visualization ptr + LocalVisualizationPtr visual_; + //! frame to visualization + std::string visual_frame_; + //! ros publisher + ros::Publisher vel_pub_; + //! When no global planner give the global plan, use local goal express robot end point + geometry_msgs::PoseStamped local_goal_; + //! local planner algorithm max error + int max_error_; + //! local planner condition variable + std::condition_variable plan_condition_; + //! local planner mutex + std::mutex plan_mutex_; + //! control frequency + double frequency_; + +}; + +} // namespace roborts_local_planner + +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_NODE_H diff --git a/roborts_planning/local_planner/include/local_planner/local_visualization.h b/roborts_planning/local_planner/include/local_planner/local_visualization.h new file mode 100644 index 00000000..0c57d564 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/local_visualization.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_LOCAL_VISUALIZATION_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_LOCAL_VISUALIZATION_H + +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "timed_elastic_band/teb_vertex_console.h" + + +namespace roborts_local_planner { + +/** + * @brief Class use to visualize local planner algorithm's trajectory + */ +class LocalVisualization { + public: + /** + * @brief Constructor + */ + LocalVisualization(); + /** + * @brief Constructor initialize this class + * @param nh Ros node handle + * @param visualize_frame Visualize frame + */ + LocalVisualization(ros::NodeHandle& nh, const std::string& visualize_frame); + /** + * @brief Initialize visualize frame and ros param + * @param nh ros node handle + * @param visualize_frame Visualize frame + */ + void Initialization(ros::NodeHandle& nh, const std::string& visualize_frame); + /** + * @brief publish trajectory + * @param vertex_console Robot trajectory from local planner algorithm + */ + void PublishLocalPlan(const TebVertexConsole &vertex_console) const; + + protected: + + //! trajectory publisher + ros::Publisher local_planner_; + //! trajectory pose publisher + ros::Publisher pose_pub_; + + //! visualize frame + std::string visual_frame_ = "map"; + + //! initialize state + bool initialized_; + +}; + +typedef boost::shared_ptr LocalVisualizationPtr; + +} // namespace roborts_local_planner + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_LOCAL_VISUALIZATION_H + diff --git a/roborts_planning/local_planner/include/local_planner/obstacle.h b/roborts_planning/local_planner/include/local_planner/obstacle.h new file mode 100644 index 00000000..20d270ad --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/obstacle.h @@ -0,0 +1,488 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_OBSTACLES_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_OBSTACLES_H + +#include +#include + +#include + +#include +#include + +#include +#include + +#include "local_planner/distance_calculation.h" + + + +namespace roborts_local_planner { + +/** + * @brief A class use to describe obstacle + */ +class Obstacle { + public: + /** + * @brief Constructor + */ + Obstacle() : dynamic_(false), centroid_velocity_(Eigen::Vector2d::Zero()) { + } + + /** + * @brief Destructor + */ + virtual ~Obstacle() { + } + + /** + * @brief Get obstacle's center + * @return Obstacle's center + */ + virtual const Eigen::Vector2d &GetCentroid() const = 0; + + /** + *@brief Get obstacle's center + * @return Obstacle's center expressed by complex number + */ + virtual std::complex GetCentroidCplx() const = 0; + + /** + * @brief Check collision + * @param position Position where want to check collision + * @param min_dist Minimal distance between obstacle and position + * @return If the distance between obstacle and position < min_dist return true, otherwise return false. + */ + virtual bool CheckCollision(const Eigen::Vector2d &position, double min_dist) const = 0; + + /** + * @brief Check intersection + * @param line_start Line start point + * @param line_end Line end point + * @param min_dist Minimal distance between line and position + * @return If the distance between obstacle and line < min_dist return true, otherwise return false. + */ + virtual bool CheckLineIntersection(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + double min_dist = 0) const = 0; + + + /** + * @brief Calculate minimum distance + * @param position Position where want to calculate distance + * @return Minimum distance + */ + virtual double GetMinimumDistance(const Eigen::Vector2d &position) const = 0; + + /** + * @brief Calculate minimum distance + * @param line_start Line start point + * @param line_end Line end point + * @return Minimum distance + */ + virtual double GetMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const = 0; + + /** + * @brief Calculate minimum distance + * @param polygon Polygon vertices + * @return Minimum distance + */ + virtual double GetMinimumDistance(const Point2dContainer &polygon) const = 0; + + /** + * @brief Get the closet point with obstacle + * @param position Point want to calculate the closet point + * @return Closet point to the point + */ + virtual Eigen::Vector2d GetClosestPoint(const Eigen::Vector2d &position) const = 0; + + /** + * @brief Get the obstacle whether or not dynamic + * @return Return true if the obstacle is dynamic, otherwise is false + */ + bool IsDynamic() const { return dynamic_; } + + /** + * @brief Set velocity of the obstacle + * @param vel Center velocity of the obstacle + */ + void SetCentroidVelocity(const Eigen::Ref &vel) { + centroid_velocity_ = vel; + dynamic_ = true; + } + + /** + * @brief Get obstacle velocity + * @return Obstacle's velocity + */ + const Eigen::Vector2d &GetCentroidVelocity() const { return centroid_velocity_; } + + /** + * @brief Convert obstacle to geometry_msgs::Polygon type + * @param polygon Input and output, result after convert. + */ + virtual void ToPolygonMsg(geometry_msgs::Polygon &polygon) = 0; + + protected: + + //! If true obstacle is dynamic, otherwise is false + bool dynamic_; + //! Velocity of obstacle + Eigen::Vector2d centroid_velocity_; + + public: + //! Make aligned memory + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +typedef boost::shared_ptr ObstaclePtr; + +typedef boost::shared_ptr ObstacleConstPtr; + +typedef std::vector ObstContainer; + +/** + * @brief Point obstacle + */ +class PointObstacle : public Obstacle { + public: + + PointObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero()) {} + + PointObstacle(const Eigen::Ref &position) : Obstacle(), pos_(position) {} + + PointObstacle(double x, double y) : Obstacle(), pos_(Eigen::Vector2d(x, y)) {} + + virtual bool CheckCollision(const Eigen::Vector2d &point, double min_dist) const { + return GetMinimumDistance(point) < min_dist; + } + + virtual bool CheckLineIntersection(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + double min_dist = 0) const { + + Eigen::Vector2d a = line_end - line_start; + Eigen::Vector2d b = pos_ - line_start; + + double t = a.dot(b) / a.dot(a); + if (t < 0) t = 0; + else if (t > 1) t = 1; + Eigen::Vector2d nearest_point = line_start + a * t; + + return CheckCollision(nearest_point, min_dist); + } + + virtual double GetMinimumDistance(const Eigen::Vector2d &position) const { + return (position - pos_).norm(); + } + + virtual double GetMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const { + return DistancePointToSegment2D(pos_, line_start, line_end); + } + + virtual double GetMinimumDistance(const Point2dContainer &polygon) const { + return DistancePointToPolygon2D(pos_, polygon); + } + + virtual Eigen::Vector2d GetClosestPoint(const Eigen::Vector2d &position) const { + return pos_; + } + + virtual const Eigen::Vector2d &GetCentroid() const { + return pos_; + } + + virtual std::complex GetCentroidCplx() const { + return std::complex(pos_[0], pos_[1]); + } + + const Eigen::Vector2d &Position() const { + return pos_; + } + Eigen::Vector2d &Position() { + return pos_; + } + + virtual void ToPolygonMsg(geometry_msgs::Polygon &polygon) { + polygon.points.resize(1); + polygon.points.front().x = pos_.x(); + polygon.points.front().y = pos_.y(); + polygon.points.front().z = 0; + } + + protected: + + Eigen::Vector2d pos_; + + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/** + * @brief Line obstacle + */ +class LineObstacle : public Obstacle { + public: + typedef std::vector > VertexContainer; + + LineObstacle() : Obstacle() { + start_.setZero(); + end_.setZero(); + centroid_.setZero(); + } + + LineObstacle(const Eigen::Ref &line_start, const Eigen::Ref &line_end) + : Obstacle(), start_(line_start), end_(line_end) { + CalcCentroid(); + } + + LineObstacle(double x1, double y1, double x2, double y2) : Obstacle() { + start_.x() = x1; + start_.y() = y1; + end_.x() = x2; + end_.y() = y2; + CalcCentroid(); + } + + virtual bool CheckCollision(const Eigen::Vector2d &point, double min_dist) const { + return GetMinimumDistance(point) <= min_dist; + } + + virtual bool CheckLineIntersection(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + double min_dist = 0) const { + return CheckLineSegmentsIntersection2D(line_start, line_end, start_, end_); + } + + virtual double GetMinimumDistance(const Eigen::Vector2d &position) const { + return DistancePointToSegment2D(position, start_, end_); + } + + virtual double GetMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const { + return DistanceSegmentToSegment2D(start_, end_, line_start, line_end); + } + + virtual double GetMinimumDistance(const Point2dContainer &polygon) const { + return DistanceSegmentToPolygon2D(start_, end_, polygon); + } + + virtual Eigen::Vector2d GetClosestPoint(const Eigen::Vector2d &position) const { + return ClosestPointOnLineSegment2D(position, start_, end_); + } + + virtual const Eigen::Vector2d &GetCentroid() const { + return centroid_; + } + + virtual std::complex GetCentroidCplx() const { + return std::complex(centroid_.x(), centroid_.y()); + } + + const Eigen::Vector2d &Start() const { + return start_; + } + void SetStart(const Eigen::Ref &start) { + start_ = start; + CalcCentroid(); + } + const Eigen::Vector2d &End() const { + return end_; + } + void SetEnd(const Eigen::Ref &end) { + end_ = end; + CalcCentroid(); + } + + virtual void ToPolygonMsg(geometry_msgs::Polygon &polygon) { + polygon.points.resize(2); + polygon.points.front().x = start_.x(); + polygon.points.front().y = start_.y(); + + polygon.points.back().x = end_.x(); + polygon.points.back().y = end_.y(); + polygon.points.back().z = polygon.points.front().z = 0; + } + + protected: + void CalcCentroid() { + centroid_ = 0.5 * (start_ + end_); + } + + private: + Eigen::Vector2d start_; + Eigen::Vector2d end_; + + Eigen::Vector2d centroid_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/** + * @brief Polygon obstacle + */ +class PolygonObstacle : public Obstacle { + public: + + PolygonObstacle() : Obstacle(), finalized_(false) { + centroid_.setConstant(NAN); + } + + PolygonObstacle(const Point2dContainer &vertices) : Obstacle(), vertices_(vertices) { + FinalizePolygon(); + } + + virtual bool CheckCollision(const Eigen::Vector2d &point, double min_dist) const { + if (NoVertices() == 2) + return GetMinimumDistance(point) <= min_dist; + + int i, j; + bool c = false; + for (i = 0, j = NoVertices() - 1; i < NoVertices(); j = i++) { + if (((vertices_.at(i).y() > point.y()) != (vertices_.at(j).y() > point.y())) && + (point.x() < (vertices_.at(j).x() - vertices_.at(i).x()) * (point.y() - vertices_.at(i).y()) + / (vertices_.at(j).y() - vertices_.at(i).y()) + vertices_.at(i).x())) { + c = !c; + } + } + if (c > 0) { + return true; + } + + return min_dist == 0 ? false : GetMinimumDistance(point) < min_dist; + } + + virtual bool CheckLineIntersection(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + double min_dist = 0) const; + + virtual double GetMinimumDistance(const Eigen::Vector2d &position) const { + return DistancePointToPolygon2D(position, vertices_); + } + + virtual double GetMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const { + return DistanceSegmentToPolygon2D(line_start, line_end, vertices_); + } + + virtual double GetMinimumDistance(const Point2dContainer &polygon) const { + return DistancePolygonToPolygon2D(polygon, vertices_); + } + + virtual Eigen::Vector2d GetClosestPoint(const Eigen::Vector2d &position) const; + + virtual const Eigen::Vector2d &GetCentroid() const { + ROS_ASSERT_MSG(finalized_, "Finalize the polygon after all vertices are added."); + return centroid_; + } + + virtual std::complex GetCentroidCplx() const { + ROS_ASSERT_MSG(finalized_, "Finalize the polygon after all vertices are added."); + return std::complex(centroid_.coeffRef(0), centroid_.coeffRef(1)); + } + + virtual void ToPolygonMsg(geometry_msgs::Polygon &polygon); + + const Point2dContainer &Vertices() const { + return vertices_; + } + + Point2dContainer &Vertices() { + return vertices_; + } + + void PushBackVertex(const Eigen::Ref &vertex) { + vertices_.push_back(vertex); + finalized_ = false; + } + + void FinalizePolygon() { + FixPolygonClosure(); + CalcCentroid(); + finalized_ = true; + } + + void ClearVertices() { + vertices_.clear(); + finalized_ = false; + } + + int NoVertices() const { + return (int) vertices_.size(); + } + + protected: + + void FixPolygonClosure(); + + void CalcCentroid(); + + + Point2dContainer vertices_; + Eigen::Vector2d centroid_; + + bool finalized_; + + + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_OBSTACLES_H diff --git a/roborts_planning/local_planner/include/local_planner/odom_info.h b/roborts_planning/local_planner/include/local_planner/odom_info.h new file mode 100644 index 00000000..3292c227 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/odom_info.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_ODOM_INFO_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_ODOM_INFO_H + +#include + +#include + +#include +#include +#include +#include + + +namespace roborts_local_planner { + +/** + * @brief A class of odometry + */ +class OdomInfo { + public: + + /** + * Constructor + * @param topic Topic of odom info, default is null + */ + OdomInfo(std::string topic = ""); + ~OdomInfo() {} + + /** + * @brief Odom's callback function + * @param msg Odom's messages + */ + void OdomCB(const nav_msgs::Odometry::ConstPtr& msg); + + /** + * @brief Get velocity + * @param vel Velocity + */ + void GetVel(tf::Stamped& vel); + + /** + * @brief Set Odom callback + * @param topic topic of odom info + */ + void SetTopic(std::string topic); + + /** + * @brief Get odom topic + * @return Odom topic name + */ + std::string GetTopic() const { return topic_; } + + private: + + //! odom topic name + std::string topic_; + //! subscriber + ros::Subscriber sub_; + //! odom info + nav_msgs::Odometry odom_; + //! odom mutex + boost::mutex mutex_; + //! odom frame id + std::string frame_id_; + +}; + +} // namespace roborts_local_planner +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_ODOM_INFO_H diff --git a/roborts_planning/local_planner/include/local_planner/optimal_base.h b/roborts_planning/local_planner/include/local_planner/optimal_base.h new file mode 100644 index 00000000..30d4820b --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/optimal_base.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_OPTIMAL_BASE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_OPTIMAL_BASE_H + +#include + +#include +#include +#include +#include + +#include "local_planner/data_base.h" +#include "state/error_code.h" +#include "local_planner/robot_position_cost.h" + +namespace roborts_local_planner { + +/** + * @brief Base class of optimal based planner algorithm + */ +class OptimalBase { + public: + /** + * @brief Constructor + */ + OptimalBase(){ + + } + /** + * @brief Entry function of optimal based planner algorithm + * @param initial_plan Input and output, way point want to optimal + * @param start_vel Start velocity of robot + * @param free_goal_vel If true set the goal velocity to zero + * @param micro_control If true when the goal behind the robot, robot will have negative x velocity + * @return True if success, otherwise false + */ + virtual bool Optimal(std::vector& initial_plan, const geometry_msgs::Twist* start_vel = NULL, + bool free_goal_vel = false, bool micro_control = false) = 0; + + /** + * @brief Entry function of optimal based planner algorithm + * @param start Start position (maybe robot current position) + * @param goal Goal position (where robot want to go) + * @param start_vel Robot current velocity + * @param free_goal_vel If true set the goal velocity to zero + * @param micro_control If true when the goal behind the robot, robot will have negative x velocity + * @return True if success, otherwise false + */ + virtual bool Optimal(const DataBase& start, const DataBase& goal, const geometry_msgs::Twist* start_vel = NULL, + bool free_goal_vel = false, bool micro_control = false) = 0; + + /** + * @brief Get next control velocity + * @param error_info Error info when get the robot velocity + * @param vx X component velocity + * @param vy Y component velocity + * @param omega Angular velocity + * @param acc_x X component acceleration + * @param acc_y Y component acceleration + * @param acc_omega Angular acceleration + * @return True if success, otherwise false + */ + virtual bool GetVelocity(roborts_common::ErrorInfo &error_info, double& vx, double& vy, double& omega, + double& acc_x, double& acc_y, double& acc_omega) const = 0; + + /** + * @brief Clear all vertices and edges + */ + virtual void ClearPlanner() = 0; + + /** + * @brief Visualize trajectory and pose after optimize + */ + virtual void Visualize() { + + } + + /** + * @brief Judge if the trajectory after optimize is feasible + * @param error_info Error info about trajectory feasible judge + * @param position_cost Cost map which to calculate the point cost + * @param footprint_spec Robot vertices + * @param inscribed_radius Robot inscribed radius + * @param circumscribed_radius Robot circumscribed radius + * @param look_ahead_idx How many point want to check + * @return True if feasible, otherwise is false + */ + virtual bool IsTrajectoryFeasible(roborts_common::ErrorInfo &error_info, RobotPositionCost* position_cost, + const std::vector& footprint_spec, + double inscribed_radius = 0.0, double circumscribed_radius=0.0, + int look_ahead_idx = -1) = 0; + + /** + *@brief Now we not use + */ + virtual bool IsHorizonReductionAppropriate(const std::vector& initial_plan) const { + return false; + } + + /** + * @brief Compute current graph cost(errors) + * @param cost Input and output, total errors of all edges + * @param obst_cost_scale Obstacle's scale number + * @param alternative_time_cost True use trajectory time diff add to errors + */ + virtual void ComputeCurrentCost(std::vector& cost, double obst_cost_scale = 1.0, bool alternative_time_cost = false) { + + } + + /** + * @brief Destructor + */ + virtual ~OptimalBase(){ + + } + + +}; + +typedef boost::shared_ptr OptimalBasePtr; + +} // namespace roborts_local_planner + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_OPTIMAL_BASE_H \ No newline at end of file diff --git a/roborts_planning/local_planner/include/local_planner/proto/local_planner.proto b/roborts_planning/local_planner/include/local_planner/proto/local_planner.proto new file mode 100644 index 00000000..13b65ed0 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/proto/local_planner.proto @@ -0,0 +1,8 @@ +syntax = "proto2"; +package roborts_local_planner; + +message LocalAlgorithms { + repeated string name = 1; + optional string selected_algorithm = 2; + optional double frequency = 3; +} diff --git a/roborts_planning/local_planner/include/local_planner/robot_footprint_model.h b/roborts_planning/local_planner/include/local_planner/robot_footprint_model.h new file mode 100644 index 00000000..72d5a4e0 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/robot_footprint_model.h @@ -0,0 +1,300 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_ROBOT_FOOTPRINT_MODEL_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_ROBOT_FOOTPRINT_MODEL_H + +#include "local_planner/data_base.h" +#include "local_planner/obstacle.h" +#include "local_planner/distance_calculation.h" + +namespace roborts_local_planner { + +/** + * @brief A class to describe robot footprint + */ +class BaseRobotFootprintModel { + public: + + /** + * @brief Constructor + */ + BaseRobotFootprintModel() { + } + + /** + * @brief Destructor + */ + virtual ~BaseRobotFootprintModel() { + } + + /** + * @brief Calculate distance + * @param current_pose Current robot pose + * @param obstacle Obstacle want to calculate + * @return Distance from current pose to obstalce + */ + virtual double CalculateDistance(const DataBase ¤t_pose, const Obstacle *obstacle) const = 0; + + /** + * @brief Get robot inscribed radius + * @return Inscribed radius + */ + virtual double GetInscribedRadius() = 0; + + public: + //! make aligned memory + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +typedef boost::shared_ptr RobotFootprintModelPtr; + +typedef boost::shared_ptr RobotFootprintModelConstPtr; + +/** + * @brief Point footprint + */ +class PointRobotFootprint : public BaseRobotFootprintModel { + public: + + PointRobotFootprint() {} + + virtual ~PointRobotFootprint() {} + + virtual double CalculateDistance(const DataBase ¤t_pose, const Obstacle *obstacle) const { + return obstacle->GetMinimumDistance(current_pose.GetPosition()); + } + + virtual double GetInscribedRadius() { return 0.0; } + +}; + +/** + * @brief Circular footprint + */ +class CircularRobotFootprint : public BaseRobotFootprintModel { + public: + + CircularRobotFootprint(double radius) : radius_(radius) {} + + virtual ~CircularRobotFootprint() {} + + void SetRadius(double radius) { radius_ = radius; } + + virtual double CalculateDistance(const DataBase ¤t_pose, const Obstacle *obstacle) const { + return obstacle->GetMinimumDistance(current_pose.GetPosition()) - radius_; + } + + + virtual double GetInscribedRadius() { return radius_; } + + private: + + double radius_; +}; + +/** + * @brief Two circles footprint + */ +class TwoCirclesRobotFootprint : public BaseRobotFootprintModel { + public: + + TwoCirclesRobotFootprint(double front_offset, double front_radius, double rear_offset, double rear_radius) + : front_offset_(front_offset), + front_radius_(front_radius), + rear_offset_(rear_offset), + rear_radius_(rear_radius) {} + + virtual ~TwoCirclesRobotFootprint() {} + + void SetParameters(double front_offset, double front_radius, double rear_offset, double rear_radius) { + front_offset_ = front_offset; + front_radius_ = front_radius; + rear_offset_ = rear_offset; + rear_radius_ = rear_radius; + } + + virtual double CalculateDistance(const DataBase ¤t_pose, const Obstacle *obstacle) const { + Eigen::Vector2d dir = current_pose.OrientationUnitVec(); + double dist_front = obstacle->GetMinimumDistance(current_pose.GetPosition() + front_offset_ * dir) - front_radius_; + double dist_rear = obstacle->GetMinimumDistance(current_pose.GetPosition() - rear_offset_ * dir) - rear_radius_; + return std::min(dist_front, dist_rear); + } + + virtual double GetInscribedRadius() { + double min_longitudinal = std::min(rear_offset_ + rear_radius_, front_offset_ + front_radius_); + double min_lateral = std::min(rear_radius_, front_radius_); + return std::min(min_longitudinal, min_lateral); + } + + private: + + double front_offset_; + double front_radius_; + double rear_offset_; + double rear_radius_; + +}; + +/** + * @brief Line footprint + */ +class LineRobotFootprint : public BaseRobotFootprintModel { + public: + + LineRobotFootprint(const geometry_msgs::Point &line_start, const geometry_msgs::Point &line_end) { + SetLine(line_start, line_end); + } + + LineRobotFootprint(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) { + SetLine(line_start, line_end); + } + + virtual ~LineRobotFootprint() {} + + void SetLine(const geometry_msgs::Point &line_start, const geometry_msgs::Point &line_end) { + line_start_.x() = line_start.x; + line_start_.y() = line_start.y; + line_end_.x() = line_end.x; + line_end_.y() = line_end.y; + } + + void SetLine(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) { + line_start_ = line_start; + line_end_ = line_end; + } + + virtual double CalculateDistance(const DataBase ¤t_pose, const Obstacle *obstacle) const { + // here we are doing the transformation into the world frame manually + double cos_th = std::cos(current_pose.GetTheta()); + double sin_th = std::sin(current_pose.GetTheta()); + Eigen::Vector2d line_start_world; + line_start_world.x() = current_pose.GetPosition().coeffRef(0) + cos_th * line_start_.x() - sin_th * line_start_.y(); + line_start_world.y() = current_pose.GetPosition().coeffRef(1) + sin_th * line_start_.x() + cos_th * line_start_.y(); + Eigen::Vector2d line_end_world; + line_end_world.x() = current_pose.GetPosition().coeffRef(0) + cos_th * line_end_.x() - sin_th * line_end_.y(); + line_end_world.y() = current_pose.GetPosition().coeffRef(1) + sin_th * line_end_.x() + cos_th * line_end_.y(); + return obstacle->GetMinimumDistance(line_start_world, line_end_world); + } + + virtual double GetInscribedRadius() { + return 0.0; // lateral distance = 0.0 + } + + private: + + Eigen::Vector2d line_start_; + Eigen::Vector2d line_end_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +/** + * @brief Polygon footprint + */ +class PolygonRobotFootprint : public BaseRobotFootprintModel { + public: + + PolygonRobotFootprint(const Point2dContainer &vertices) : vertices_(vertices) {} + + virtual ~PolygonRobotFootprint() {} + + void SetVertices(const Point2dContainer &vertices) { vertices_ = vertices; } + + virtual double CalculateDistance(const DataBase ¤t_pose, const Obstacle *obstacle) const { + // here we are doing the transformation into the world frame manually + double cos_th = std::cos(current_pose.GetTheta()); + double sin_th = std::sin(current_pose.GetTheta()); + + Point2dContainer polygon_world(vertices_.size()); + for (std::size_t i = 0; i < vertices_.size(); ++i) { + polygon_world[i].x() = current_pose.GetPosition().coeffRef(0) + cos_th * vertices_[i].x() - sin_th * vertices_[i].y(); + polygon_world[i].y() = current_pose.GetPosition().coeffRef(1) + sin_th * vertices_[i].x() + cos_th * vertices_[i].y(); + + } + return obstacle->GetMinimumDistance(polygon_world); + } + + virtual double GetInscribedRadius() { + double min_dist = std::numeric_limits::max(); + Eigen::Vector2d center(0.0, 0.0); + + if (vertices_.size() <= 2) + return 0.0; + + for (int i = 0; i < (int) vertices_.size() - 1; ++i) { + // compute distance from the robot center point to the first vertex + double vertex_dist = vertices_[i].norm(); + double edge_dist = DistancePointToSegment2D(center, vertices_[i], vertices_[i + 1]); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + } + + // we also need to check the last vertex and the first vertex + double vertex_dist = vertices_.back().norm(); + double edge_dist = DistancePointToSegment2D(center, vertices_.back(), vertices_.front()); + return std::min(min_dist, std::min(vertex_dist, edge_dist)); + } + + private: + + Point2dContainer vertices_; + +}; + +} // namespace roborts_local_planner + +#endif /* ROBOT_FOOTPRINT_MODEL_H */ diff --git a/roborts_planning/local_planner/include/local_planner/robot_position_cost.h b/roborts_planning/local_planner/include/local_planner/robot_position_cost.h new file mode 100644 index 00000000..71cfe6c9 --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/robot_position_cost.h @@ -0,0 +1,214 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_ROBOT_POSITION_COST_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_ROBOT_POSITION_COST_H + +#include + +#include +#include + +#include "costmap/costmap_interface.h" +#include "local_planner/utility_tool.h" +#include "local_planner/line_iterator.h" +#include "local_planner/robot_footprint_model.h" + +namespace roborts_local_planner { + +static const unsigned char NO_INFORMATION = 255; +static const unsigned char LETHAL_OBSTACLE = 254; +static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; +static const unsigned char FREE_SPACE = 0; + +/** + * @brief Calculate each robot vertices cost + */ +class RobotPositionCost { + public: + /** + * @brief Constructor + * @param cost_map Cost map use to calculate cost + */ + RobotPositionCost(const roborts_costmap::Costmap2D& cost_map); + /** + * @brief Destructor + */ + ~RobotPositionCost(); + + /** + * @brief Calculate minimum and maximum distance + * @param footprint Robot vertices + * @param min_dist Input and output, minimum distance + * @param max_dist Input and output, maximum distance + */ + static void CalculateMinAndMaxDistances (const std::vector& footprint, double& min_dist, double& max_dist) { + min_dist = std::numeric_limits::max(); + max_dist = 0.0; + + if (footprint.size() <= 2) + { + return; + } + + for (unsigned int i = 0; i < footprint.size() - 1; ++i) { + double vertex_dist = Distance (0.0, 0.0, footprint[i].coeffRef(0), footprint[i].coeffRef(1)); + double edge_dist = PointToLineDistance(0.0, 0.0, footprint[i].coeffRef(0), footprint[i].coeffRef(1), + footprint[i + 1].coeffRef(0), footprint[i + 1].coeffRef(1)); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); + } + + double vertex_dist = Distance(0.0, 0.0, footprint.back().coeffRef(0), footprint.back().coeffRef(1)); + double edge_dist = PointToLineDistance(0.0, 0.0, footprint.back().coeffRef(0), footprint.back().coeffRef(1), + footprint.front().coeffRef(0), footprint.front().coeffRef(1)); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); + } + + /** + * @brief Calculate robot vertices current cost + * @param position Robot current pose + * @param footprint Robot vertices + * @return Robot vertices maximum cost, if < 0, can not get the cost + */ + double FootprintCost (const Eigen::Vector2d& position, const std::vector& footprint); + + /** + * @brief Calculate robot vertices current cost + * @param x Robot current x + * @param y Robot current y + * @param theta Robot current theta + * @param footprint_spec Robot vertices + * @param inscribed_radius Robot inscribed radius + * @param circumscribed_radius Robot circumscribed radius + * @return Robot vertices maximum cost, if < 0, can not get the cost + */ + double FootprintCost (double x, double y, double theta, + const std::vector& footprint_spec, + double inscribed_radius = 0.0, double circumscribed_radius=0.0); + + /** + * @brief A line max cost in cost map + * @param x0 Line start x + * @param x1 Line end x + * @param y0 Line start y + * @param y1 Line end y + * @return Line maximum cost + */ + double LineCost (int x0, int x1, int y0, int y1); + + /** + * @brief A point cost in cost map + * @param x Point x + * @param y Point y + * @return Point cost + */ + double PointCost(int x, int y); + + private: + //! Cost map + const roborts_costmap::Costmap2D& costmap_; +}; + +/** + * @brief Get robot type(Point, circular, two circles, line, polygon) + * @tparam T Robot type class + * @param config robot type param + * @return Robot footprint model + */ +template +RobotFootprintModelPtr GetRobotFootprintModel (T config) { + if (!config.robot_type(0).has_type()) { + ROS_INFO("No robot footprint model specified. Using point-shaped model."); + return boost::make_shared(); + } else if (config.robot_type(0).type() == 0) { // point type + ROS_INFO("Footprint model 'point' loaded"); + return boost::make_shared(); + } else if (config.robot_type(0).type() == 1) {// circular + // get radius + double radius; + if (!config.robot_type(0).has_radius()) { + ROS_INFO("the circle radius doesn't exist, Using point-shaped model"); + return boost::make_shared(); + } + radius = config.robot_type(0).radius(); + ROS_INFO("Footprint model 'circular' (radius: %f m) loaded", radius); + return boost::make_shared(radius); + } else if (config.robot_type(0).type() == 3) { // line + if (! config.robot_type(0).robot_vertices_size()) { + ROS_INFO("the line vertices don't exist, Using point-shaped model"); + return boost::make_shared(); + } + // get line coordinates + Eigen::Vector2d line_start, line_end; + line_start.coeffRef(0) = config.robot_type(0).robot_vertices(0).x(); + line_start.coeffRef(1) = config.robot_type(0).robot_vertices(0).y(); + + line_end.coeffRef(0) = config.robot_type(0).robot_vertices(1).x(); + line_end.coeffRef(1) = config.robot_type(0).robot_vertices(1).y(); + + if (config.robot_type(0).robot_vertices_size() != 2) { + ROS_INFO("the vertices' size doesn't equal 2, Using point-shaped model"); + return boost::make_shared(); + } + + ROS_INFO("Footprint model 'line' loaded"); + return boost::make_shared(line_start, line_end); + } else if (config.robot_type(0).type() == 2) { + + if (!config.robot_type(0).front_offset()|| !config.robot_type(0).front_radius() + || !config.robot_type(0).rear_offset() || !config.robot_type(0).rear_radius() ) { + ROS_INFO("the param is not enough to combined two circle, Using point-shaped model"); + return boost::make_shared(); + } + double front_offset, front_radius, rear_offset, rear_radius; + front_offset = config.robot_type(0).front_offset(); + front_radius = config.robot_type(0).front_radius(); + rear_offset = config.robot_type(0).rear_offset(); + rear_radius = config.robot_type(0).rear_radius(); + + ROS_INFO("Footprint model 'two circle' loaded"); + return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); + } else if ( config.robot_type(0).type() == 4) { + + if (!config.robot_type(0).robot_vertices_size() ) { + ROS_INFO("the vertices is null, Using point-shaped model"); + return boost::make_shared(); + } + Point2dContainer polygon; + for (int j = 0; j < config.robot_type(0).robot_vertices_size(); ++j) { + Eigen::Vector2d temp_point; + temp_point.coeffRef(0) = config.robot_type(0).robot_vertices(j).x(); + temp_point.coeffRef(1) = config.robot_type(0).robot_vertices(j).y(); + polygon.push_back(temp_point); + } + + ROS_INFO("Footprint model 'polygon' loaded"); + return boost::make_shared(polygon); + } else { + ROS_INFO("this type doesn't exist, Using point-shaped model"); + return boost::make_shared(); + } + +} + + +} // namespace roborts_local_planner + +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_ROBOT_POSITION_COST_H + diff --git a/roborts_planning/local_planner/include/local_planner/utility_tool.h b/roborts_planning/local_planner/include/local_planner/utility_tool.h new file mode 100644 index 00000000..4846cd5c --- /dev/null +++ b/roborts_planning/local_planner/include/local_planner/utility_tool.h @@ -0,0 +1,131 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_UTILITY_TOOLS_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_UTILITY_TOOLS_H + +#include +#include + +#include +#include +#include + +namespace roborts_local_planner { + +/** + * @brief some common functions + */ +#define SMALL_NUM 0.00000001 + +enum class RotType { + LEFT, + NONE, + RIGHT +}; + +inline double LogisticSigmoid (double x) { + return x / (1 + fabs(x)); +} + +inline double Cross2D (Eigen::Vector2d v1, Eigen::Vector2d v2) { + return v1.x()*v2.y() - v2.x()*v1.y(); +} + +inline double Distance (double x0, double y0, double x1, double y1) { + return hypot(x1 - x0, y1 - y0); +} + +inline double PointToLineDistance (double px, double py, double x0, double y0, double x1, double y1) { + double A = px - x0; + double B = py - y0; + double C = x1 - x0; + double D = y1 - y0; + + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; + + double xx, yy; + + if (param < 0) + { + xx = x0; + yy = y0; + } + else if (param > 1) + { + xx = x1; + yy = y1; + } + else + { + xx = x0 + param * C; + yy = y0 + param * D; + } + + return Distance(px, py, xx, yy); +} + +inline double AverageAngles(const std::vector& angles) { + double x=0, y=0; + for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) { + x += cos(*it); + y += sin(*it); + } + if(x == 0 && y == 0){ + return 0; + } else { + return std::atan2(y, x); + } +} + +inline double GetOrientation (Eigen::Vector2d vec) { + return atan2(vec.coeffRef(1), vec.coeffRef(0)); +} + +inline std::vector EulerToQuaternion(const double& roll, const double& pitch, const double& yaw) { + double half_roll = roll * 0.5; + double half_pitch = pitch * 0.5; + double half_yaw = yaw * 0.5; + + double cos_roll = cosf(roll); + double sin_roll = sinf(roll); + + double cos_pitch = cosf(pitch); + double sin_pitch = sinf(pitch); + + double cos_yaw = cosf(yaw); + double sin_yaw = sinf(yaw); + + auto w_q0 = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw; + auto x_q1 = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw; + auto y_q2 = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw; + auto z_q3 = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw; + + std::vector Quaternion; + Quaternion.push_back(w_q0); + Quaternion.push_back(x_q1); + Quaternion.push_back(y_q2); + Quaternion.push_back(z_q3); + return Quaternion; + +} + +} // namespace roborts_local_planner + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_UTILITY_TOOLS_H diff --git a/roborts_planning/local_planner/src/local_planner_node.cpp b/roborts_planning/local_planner/src/local_planner_node.cpp new file mode 100644 index 00000000..9763ab99 --- /dev/null +++ b/roborts_planning/local_planner/src/local_planner_node.cpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include + +#include "local_planner/local_planner_node.h" + +namespace roborts_local_planner { + +using roborts_common::NodeState; +LocalPlannerNode::LocalPlannerNode() : + local_planner_nh_("~"), + as_(local_planner_nh_, "/local_planner_node_action", boost::bind(&LocalPlannerNode::ExcuteCB, this, _1), false), + initialized_(false), node_state_(roborts_common::NodeState::IDLE), + node_error_info_(roborts_common::ErrorCode::OK), max_error_(5), + local_cost_(nullptr), tf_(nullptr) { + if (Init().IsOK()) { + ROS_INFO("local planner initialize completed."); + } else { + ROS_WARN("local planner initialize failed."); + SetNodeState(NodeState::FAILURE); + } + as_.start(); +} + +LocalPlannerNode::~LocalPlannerNode() { + StopPlanning(); +} + +roborts_common::ErrorInfo LocalPlannerNode::Init() { + ROS_INFO("local planner start"); + LocalAlgorithms local_algorithms; + std::string full_path = ros::package::getPath("roborts_planning") + "/local_planner/config/local_planner.prototxt"; + roborts_common::ReadProtoFromTextFile(full_path.c_str(), &local_algorithms); + if (&local_algorithms == nullptr) { + return roborts_common::ErrorInfo(roborts_common::ErrorCode::LP_INITILIZATION_ERROR, + "Cannot load local planner protobuf configuration file."); + } + selected_algorithm_ = local_algorithms.selected_algorithm(); + frequency_ = local_algorithms.frequency(); + tf_ = std::make_shared(ros::Duration(10)); + + std::string map_path = ros::package::getPath("roborts_costmap") + \ + "/config/costmap_parameter_config_for_local_plan.prototxt"; + local_cost_ = std::make_shared("local_costmap", + *tf_, + map_path.c_str()); + local_planner_ = roborts_common::AlgorithmFactory::CreateAlgorithm(selected_algorithm_); + if (local_planner_== nullptr) { + ROS_ERROR("global planner algorithm instance can't be loaded"); + return roborts_common::ErrorInfo(roborts_common::ErrorCode::LP_INITILIZATION_ERROR, + "local planner algorithm instance can't be loaded"); + } + + std::string name; + visual_frame_ = local_cost_->GetGlobalFrameID(); + visual_ = LocalVisualizationPtr(new LocalVisualization(local_planner_nh_, visual_frame_)); + vel_pub_ = local_planner_nh_.advertise("/cmd_vel_acc", 5); + + return roborts_common::ErrorInfo(roborts_common::ErrorCode::OK); +} + +void LocalPlannerNode::ExcuteCB(const roborts_msgs::LocalPlannerGoal::ConstPtr &command) { + roborts_common::ErrorInfo error_info = GetErrorInfo(); + NodeState node_state = GetNodeState(); + + if (node_state == NodeState::FAILURE) { + roborts_msgs::LocalPlannerFeedback feedback; + roborts_msgs::LocalPlannerResult result; + feedback.error_code = error_info.error_code(); + feedback.error_msg = error_info.error_msg(); + result.error_code = feedback.error_code; + as_.publishFeedback(feedback); + as_.setAborted(result, feedback.error_msg); + ROS_ERROR("Initialization Failed, Failed to execute action!"); + return; + } + if (plan_mtx_.try_lock()) { + local_planner_->SetPlan(command->route, local_goal_); + plan_mtx_.unlock(); + plan_condition_.notify_one(); + } + + ROS_INFO("Send Plan!"); + if (node_state == NodeState::IDLE) { + StartPlanning(); + } + + while (ros::ok()) { + std::this_thread::sleep_for(std::chrono::microseconds(1)); + + if (as_.isPreemptRequested()) { + ROS_INFO("Action Preempted"); + if (as_.isNewGoalAvailable()) { + as_.setPreempted(); + break; + } else { + as_.setPreempted(); + StopPlanning(); + break; + } + } + node_state = GetNodeState(); + error_info = GetErrorInfo(); + + if (node_state == NodeState::RUNNING|| node_state == NodeState::SUCCESS + || node_state == NodeState::FAILURE) { + roborts_msgs::LocalPlannerFeedback feedback; + roborts_msgs::LocalPlannerResult result; + if (!error_info.IsOK()) { + feedback.error_code = error_info.error_code(); + feedback.error_msg = error_info.error_msg(); + SetErrorInfo(roborts_common::ErrorInfo::OK()); + + as_.publishFeedback(feedback); + } + if(node_state == NodeState::SUCCESS) { + result.error_code = error_info.error_code(); + as_.setSucceeded(result,error_info.error_msg()); + StopPlanning(); + break; + } else if(node_state == NodeState::FAILURE) { + result.error_code = error_info.error_code(); + as_.setAborted(result,error_info.error_msg()); + StopPlanning(); + break; + } + } + } + +} + +void LocalPlannerNode::Loop() { + + roborts_common::ErrorInfo error_info = local_planner_->Initialize(local_cost_, tf_, visual_); + if (error_info.IsOK()) { + ROS_INFO("local planner algorithm initialize completed."); + } else { + ROS_WARN("local planner algorithm initialize failed."); + SetNodeState(NodeState::FAILURE); + SetErrorInfo(error_info); + } + std::chrono::microseconds sleep_time = std::chrono::microseconds(0); + int error_count = 0; + + while (GetNodeState() == NodeState::RUNNING) { + std::unique_lock plan_lock(plan_mutex_); + plan_condition_.wait_for(plan_lock, sleep_time); + auto begin = std::chrono::steady_clock::now(); + roborts_common::ErrorInfo error_info = local_planner_->ComputeVelocityCommands(cmd_vel_); + auto cost_time = std::chrono::duration_cast(std::chrono::steady_clock::now() - begin); + int need_time = 1000 /frequency_; + sleep_time = std::chrono::milliseconds(need_time) - cost_time; + + if (sleep_time <= std::chrono::milliseconds(0)) { + //LOG_WARNING << "The time planning once is " << cost_time.count() << " beyond the expected time " + // << std::chrono::milliseconds(50).count(); + sleep_time = std::chrono::milliseconds(0); + //SetErrorInfo(ErrorInfo(ErrorCode::GP_TIME_OUT_ERROR, "Planning once time out.")); + } + + if (error_info.IsOK()) { + error_count = 0; + vel_pub_.publish(cmd_vel_); + if (local_planner_->IsGoalReached()) { + SetNodeState(NodeState::SUCCESS); + } + } else if (error_count > max_error_ && max_error_ >0) { + ROS_WARN("Can not finish plan with max retries( %d )", max_error_ ); + error_info = roborts_common::ErrorInfo(roborts_common::ErrorCode::LP_MAX_ERROR_FAILURE, "over max error."); + SetNodeState(NodeState::FAILURE); + } else { + error_count++; + ROS_ERROR("Can not get cmd_vel for once. %s error count: %d", error_info.error_msg().c_str(), error_count); + } + + SetErrorInfo(error_info); + } + + cmd_vel_.twist.linear.x = 0; + cmd_vel_.twist.linear.y = 0; + cmd_vel_.twist.angular.z = 0; + + cmd_vel_.accel.linear.x = 0; + cmd_vel_.accel.linear.y = 0; + cmd_vel_.accel.angular.z = 0; +// for (int i = 0; i < 10; ++i) { + vel_pub_.publish(cmd_vel_); +// usleep(5000); +// } +} + +void LocalPlannerNode::SetErrorInfo(const roborts_common::ErrorInfo error_info) { + std::lock_guard guard(node_error_info_mtx_); + node_error_info_ = error_info; +} + +void LocalPlannerNode::SetNodeState(const roborts_common::NodeState& node_state) { + std::lock_guard guard(node_state_mtx_); + node_state_ = node_state; +} + +roborts_common::NodeState LocalPlannerNode::GetNodeState() { + std::lock_guard guard(node_state_mtx_); + return node_state_; +} + +roborts_common::ErrorInfo LocalPlannerNode::GetErrorInfo() { + std::lock_guard guard(node_error_info_mtx_); + return node_error_info_; +} + +void LocalPlannerNode::StartPlanning() { + if (local_planner_thread_.joinable()) { + local_planner_thread_.join(); + } + + SetNodeState(roborts_common::NodeState::RUNNING); + local_planner_thread_= std::thread(std::bind(&LocalPlannerNode::Loop,this)); +} + +void LocalPlannerNode::StopPlanning() { + SetNodeState(roborts_common::IDLE); + if (local_planner_thread_.joinable()) { + local_planner_thread_.join(); + } +} + +void LocalPlannerNode::AlgorithmCB(const roborts_common::ErrorInfo &algorithm_error_info) { + SetErrorInfo(algorithm_error_info); +} + +} // namespace roborts_local_planner + +void SignalHandler(int signal){ + if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ + ros::shutdown(); + } +} + +int main(int argc, char **argv) { + + signal(SIGINT, SignalHandler); + signal(SIGTERM,SignalHandler); + ros::init(argc, argv, "local_planner_node", ros::init_options::NoSigintHandler); + + roborts_local_planner::LocalPlannerNode local_planner; + + ros::AsyncSpinner async_spinner(2); + async_spinner.start(); + ros::waitForShutdown(); + local_planner.StopPlanning(); + + return 0; +} diff --git a/roborts_planning/local_planner/src/local_visualization.cpp b/roborts_planning/local_planner/src/local_visualization.cpp new file mode 100644 index 00000000..86e99747 --- /dev/null +++ b/roborts_planning/local_planner/src/local_visualization.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "local_planner/local_visualization.h" + +namespace roborts_local_planner { +LocalVisualization::LocalVisualization() : initialized_(false){ + +} +LocalVisualization::LocalVisualization(ros::NodeHandle &nh, const std::string &visualize_frame) : initialized_(false){ + Initialization(nh, visualize_frame); +} +void LocalVisualization::Initialization(ros::NodeHandle &nh, const std::string &visualize_frame) { + if (initialized_) { + + } + + visual_frame_ = visualize_frame; + local_planner_ = nh.advertise("trajectory", 1); + pose_pub_ = nh.advertise("pose", 1); + + initialized_ = true; +} + +void LocalVisualization::PublishLocalPlan(const TebVertexConsole& vertex_console) const{ + + nav_msgs::Path local_plan; + local_plan.header.frame_id = visual_frame_; + local_plan.header.stamp = ros::Time::now(); + + for (int i = 0; i . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include "local_planner/obstacle.h" + +namespace roborts_local_planner { + +void PolygonObstacle::FixPolygonClosure() { + if (vertices_.size() < 2) + return; + + if (vertices_.front().isApprox(vertices_.back())) + vertices_.pop_back(); +} + +void PolygonObstacle::CalcCentroid() { + if (vertices_.empty()) { + centroid_.setConstant(NAN); + ROS_WARN("can't compute the centriod because the vertices is null"); + return; + } + + if (NoVertices() == 1) { + centroid_ = vertices_.front(); + return; + } + + if (NoVertices() == 2) { + centroid_ = 0.5 * (vertices_.front() + vertices_.back()); + return; + } + + + centroid_.setZero(); + + // calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon) + double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i) + for (int i = 0; i < NoVertices() - 1; ++i) { + A += vertices_.at(i).coeffRef(0) * vertices_.at(i + 1).coeffRef(1) + - vertices_.at(i + 1).coeffRef(0) * vertices_.at(i).coeffRef(1); + } + A += vertices_.at(NoVertices() - 1).coeffRef(0) * vertices_.at(0).coeffRef(1) + - vertices_.at(0).coeffRef(0) * vertices_.at(NoVertices() - 1).coeffRef(1); + A *= 0.5; + + if (A != 0) { + for (int i = 0; i < NoVertices() - 1; ++i) { + double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i + 1).coeffRef(1) + - vertices_.at(i + 1).coeffRef(0) * vertices_.at(i).coeffRef(1)); + centroid_ += (vertices_.at(i) + vertices_.at(i + 1)) * aux; + } + double aux = (vertices_.at(NoVertices() - 1).coeffRef(0) * vertices_.at(0).coeffRef(1) + - vertices_.at(0).coeffRef(0) * vertices_.at(NoVertices() - 1).coeffRef(1)); + centroid_ += (vertices_.at(NoVertices() - 1) + vertices_.at(0)) * aux; + centroid_ /= (6 * A); + } else { + int i_cand = 0; + int j_cand = 0; + double min_dist = std::numeric_limits::max(); + for (int i = 0; i < NoVertices(); ++i) { + for (int j = i + 1; j < NoVertices(); ++j) { + double dist = (vertices_[j] - vertices_[i]).norm(); + if (dist < min_dist) { + min_dist = dist; + i_cand = i; + j_cand = j; + } + } + } + centroid_ = 0.5 * (vertices_[i_cand] + vertices_[j_cand]); + } +} + +Eigen::Vector2d PolygonObstacle::GetClosestPoint(const Eigen::Vector2d &position) const { + if (NoVertices() == 1) { + return vertices_.front(); + } + + if (NoVertices() > 1) { + + Eigen::Vector2d new_pt = ClosestPointOnLineSegment2D(position, vertices_.at(0), vertices_.at(1)); + + if (NoVertices() > 2) { + double dist = (new_pt - position).norm(); + Eigen::Vector2d closest_pt = new_pt; + + for (int i = 1; i < NoVertices() - 1; ++i) { + new_pt = ClosestPointOnLineSegment2D(position, vertices_.at(i), vertices_.at(i + 1)); + double new_dist = (new_pt - position).norm(); + if (new_dist < dist) { + dist = new_dist; + closest_pt = new_pt; + } + } + new_pt = ClosestPointOnLineSegment2D(position, vertices_.back(), vertices_.front()); + double new_dist = (new_pt - position).norm(); + if (new_dist < dist) + return new_pt; + else + return closest_pt; + } else { + return new_pt; + } + } + + return Eigen::Vector2d::Zero(); +} + +bool PolygonObstacle::CheckLineIntersection(const Eigen::Vector2d &line_start, + const Eigen::Vector2d &line_end, + double min_dist) const { + + for (int i = 0; i < NoVertices() - 1; ++i) { + if (CheckLineSegmentsIntersection2D(line_start, line_end, vertices_.at(i), vertices_.at(i + 1))) { + return true; + } + } + if (NoVertices() == 2) { + return false; + } + + return CheckLineSegmentsIntersection2D(line_start, + line_end, + vertices_.back(), + vertices_.front()); +} + +void PolygonObstacle::ToPolygonMsg(geometry_msgs::Polygon &polygon) { + polygon.points.resize(vertices_.size()); + for (std::size_t i = 0; i < vertices_.size(); ++i) { + polygon.points[i].x = vertices_[i].x(); + polygon.points[i].y = vertices_[i].y(); + polygon.points[i].z = 0; + } +} + +} // namespace roborts_local_planner \ No newline at end of file diff --git a/roborts_planning/local_planner/src/odom_info.cpp b/roborts_planning/local_planner/src/odom_info.cpp new file mode 100644 index 00000000..2cb331e1 --- /dev/null +++ b/roborts_planning/local_planner/src/odom_info.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "local_planner/odom_info.h" + +namespace roborts_local_planner { +OdomInfo::OdomInfo(std::string topic) { + SetTopic(topic); +} + +void OdomInfo::OdomCB(const nav_msgs::Odometry::ConstPtr& msg) { + + boost::mutex::scoped_lock lock(mutex_); + odom_.twist.twist.linear.x = msg->twist.twist.linear.x; + odom_.twist.twist.linear.y = msg->twist.twist.linear.y; + odom_.twist.twist.angular.z = msg->twist.twist.angular.z; + odom_.child_frame_id = msg->child_frame_id; + +} + +void OdomInfo::GetVel(tf::Stamped& vel) { + geometry_msgs::Twist temp_vel;{ + boost::mutex::scoped_lock lock(mutex_); + temp_vel.linear.x = odom_.twist.twist.linear.x; + temp_vel.linear.y = odom_.twist.twist.linear.y; + temp_vel.angular.z = odom_.twist.twist.angular.z; + + vel.frame_id_ = odom_.child_frame_id; + } + vel.setData(tf::Transform(tf::createQuaternionFromYaw(temp_vel.angular.z), + tf::Vector3(temp_vel.linear.x, temp_vel.linear.y, 0))); + vel.stamp_ = ros::Time(); +} + +void OdomInfo::SetTopic(std::string topic) +{ + if (topic_ == topic) { + return; + } else { + topic_ = topic; + if (topic == "") { + sub_.shutdown(); + return; + } else { + ros::NodeHandle nh; + sub_ = nh.subscribe(topic_, 1, boost::bind( &OdomInfo::OdomCB, this, _1 )); + } + } +} + +} // namespace roborts_local_planner \ No newline at end of file diff --git a/roborts_planning/local_planner/src/robot_position_cost.cpp b/roborts_planning/local_planner/src/robot_position_cost.cpp new file mode 100644 index 00000000..4a9350a1 --- /dev/null +++ b/roborts_planning/local_planner/src/robot_position_cost.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "local_planner/robot_position_cost.h" + +namespace roborts_local_planner { + +RobotPositionCost::RobotPositionCost(const roborts_costmap::Costmap2D& cost_map) : costmap_(cost_map) { + +} + +RobotPositionCost::~RobotPositionCost() { + +} + +double RobotPositionCost::PointCost(int x, int y) { + unsigned char cost = costmap_.GetCost(x, y); + if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){ + return -1; + } + + return cost; +} + +double RobotPositionCost::LineCost(int x0, int x1, int y0, int y1) { + double line_cost = 0.0; + double point_cost = -1.0; + + for(FastLineIterator line( x0, y0, x1, y1 ); line.IsValid(); line.Advance()) { + point_cost = PointCost(line.GetX(), line.GetY()); //current point's cost + + if(point_cost < 0){ + return -1; + } + + if(line_cost < point_cost){ + line_cost = point_cost; + } + + } + return line_cost; +} + +double RobotPositionCost::FootprintCost (const Eigen::Vector2d& position, const std::vector& footprint) { + + unsigned int cell_x, cell_y; + + if (!costmap_.World2Map(position.coeffRef(0), position.coeffRef(1), cell_x, cell_y)) { + return -1.0; + } + + if (footprint.size() < 3) { + unsigned char cost = costmap_.GetCost(cell_x, cell_y); + + if (cost == LETHAL_OBSTACLE || cost == NO_INFORMATION /*|| cost == INSCRIBED_INFLATED_OBSTACLE*/) { + return -1.0; + } + return cost; + } + + unsigned int x0, x1, y0, y1; + double line_cost = 0.0; + double footprint_cost = 0.0; + + for(unsigned int i = 0; i < footprint.size() - 1; ++i) { + if (!costmap_.World2Map(footprint[i].coeffRef(0), footprint[i].coeffRef(1), x0, y0)) { + return -1.0; + } + + if(!costmap_.World2Map(footprint[i + 1].coeffRef(0), footprint[i + 1].coeffRef(1), x1, y1)) { + return -1.0; + } + + line_cost = LineCost(x0, x1, y0, y1); + footprint_cost = std::max(line_cost, footprint_cost); + + if(line_cost < 0) { + return -1.0; + } + } + + if(!costmap_.World2Map(footprint.back().coeffRef(0), footprint.back().coeffRef(1), x0, y0)) { + return -1.0; + } + + if(!costmap_.World2Map(footprint.front().coeffRef(0), footprint.front().coeffRef(1), x1, y1)) { + return -1.0; + } + + line_cost = LineCost(x0, x1, y0, y1); + footprint_cost = std::max(line_cost, footprint_cost); + + if(line_cost < 0) { + return -1.0; + } + + return footprint_cost; +} + +double RobotPositionCost::FootprintCost(double x, + double y, + double theta, + const std::vector &footprint_spec, + double inscribed_radius, + double circumscribed_radius) { + double cos_th = cos(theta); + double sin_th = sin(theta); + std::vector oriented_footprint; + for (unsigned int i = 0; i < footprint_spec.size(); ++i) { + Eigen::Vector2d new_pt; + new_pt.coeffRef(0) = x + (footprint_spec[i].coeffRef(0) * cos_th - footprint_spec[i].coeffRef(1) * sin_th); + new_pt.coeffRef(1) = y + (footprint_spec[i].coeffRef(0) * sin_th + footprint_spec[i].coeffRef(1) * cos_th); + oriented_footprint.push_back(new_pt); + } + + Eigen::Vector2d robot_position; + robot_position.coeffRef(0) = x; + robot_position.coeffRef(1) = y; + + if(inscribed_radius==0.0){ + CalculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius); + } + + return FootprintCost(robot_position, oriented_footprint); +} + +} // namespace roborts_local_planner + diff --git a/roborts_planning/local_planner/src/teb_test.cpp b/roborts_planning/local_planner/src/teb_test.cpp new file mode 100644 index 00000000..8a5bdafb --- /dev/null +++ b/roborts_planning/local_planner/src/teb_test.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include +#include + +#include +#include + +#include "timed_elastic_band/teb_optimal.h" +#include "timed_elastic_band/proto/timed_elastic_band.pb.h" + +#include "local_planner/optimal_base.h" +#include "local_planner/obstacle.h" +#include "local_planner/local_visualization.h" +#include "local_planner/robot_footprint_model.h" + + + +using namespace roborts_local_planner; + +// ============= Global Variables ================ +OptimalBasePtr planner; +std::vector obst_vector; +LocalVisualizationPtr visual; +ViaPointContainer via_points; +unsigned int no_fixed_obstacles; + +// =========== Function declarations ============= +void CB_mainCycle(const ros::TimerEvent& e); +void CB_publishCycle(const ros::TimerEvent& e); +void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, + interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb); +void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + + +// =============== Main function ================= +int main( int argc, char** argv ) { + ros::init(argc, argv, "test_optim_node"); + ros::NodeHandle nh; + + roborts_local_planner::Config param_config_; + + std::string full_path = ros::package::getPath("roborts_planning") + \ + "/local_planner/timed_elastic_band/config/timed_elastic_band.prototxt"; + roborts_common::ReadProtoFromTextFile(full_path.c_str(), ¶m_config_); + ros::Timer cycle_timer = nh.createTimer(ros::Duration(0.025), CB_mainCycle); + ros::Timer publish_timer = nh.createTimer(ros::Duration(0.1), CB_publishCycle); + + interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles"); + + obst_vector.emplace_back( boost::make_shared(-3,1) ); + obst_vector.emplace_back( boost::make_shared(6,2) ); + obst_vector.emplace_back( boost::make_shared(0,0.1) ); + obst_vector.emplace_back( boost::make_shared(-4,1) ); + obst_vector.emplace_back( boost::make_shared(5,2) ); + obst_vector.emplace_back( boost::make_shared(1,0.1) ); + obst_vector.emplace_back( boost::make_shared(-3,2) ); + obst_vector.emplace_back( boost::make_shared(5,3) ); + obst_vector.emplace_back( boost::make_shared(4,0) ); + obst_vector.emplace_back( boost::make_shared(4,1) ); + obst_vector.emplace_back( boost::make_shared(3,2) ); + obst_vector.emplace_back( boost::make_shared(2,2) ); + + std::string map_frame; + for (unsigned int i=0; i pobst = boost::dynamic_pointer_cast(obst_vector.at(i)); + if (pobst) { + CreateInteractiveMarker(pobst->Position().coeff(0),pobst->Position().coeff(1), + i, "odom", &marker_server, &CB_obstacle_marker); + } + } + marker_server.applyChanges(); + + // Setup visualization + visual = LocalVisualizationPtr(new LocalVisualization(nh, "odom")); + + RobotFootprintModelPtr model = boost::make_shared(); + + planner = OptimalBasePtr(new TebOptimal(param_config_ ,&obst_vector, model, visual, &via_points)); + + + no_fixed_obstacles = (unsigned int)obst_vector.size(); + ros::spin(); + + return 0; +} + +// Planning loop +void CB_mainCycle(const ros::TimerEvent& e) { + auto start_pose = DataConverter::LocalConvertCData(-4, 0, 0); + auto end_pose = DataConverter::LocalConvertCData(4, 0, 0); + planner->Optimal(DataBase(start_pose.first, start_pose.second), DataBase(end_pose.first, end_pose.second)); +} + +// Visualization loop +void CB_publishCycle(const ros::TimerEvent& e) { + planner->Visualize(); +} + +void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, + interactive_markers::InteractiveMarkerServer* marker_server, + interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb) { + // create an interactive marker for our server + visualization_msgs::InteractiveMarker i_marker; + i_marker.header.frame_id = frame; + i_marker.header.stamp = ros::Time::now(); + std::ostringstream oss; + //oss << "obstacle" << id; + oss << id; + i_marker.name = oss.str(); + i_marker.description = "Obstacle"; + i_marker.pose.position.x = init_x; + i_marker.pose.position.y = init_y; + + // create a grey box marker + visualization_msgs::Marker box_marker; + box_marker.type = visualization_msgs::Marker::CUBE; + box_marker.id = id; + box_marker.scale.x = 0.2; + box_marker.scale.y = 0.2; + box_marker.scale.z = 0.2; + box_marker.color.r = 100; + box_marker.color.g = 0.5; + box_marker.color.b = 0.5; + box_marker.color.a = 1.0; + + // create a non-interactive control which contains the box + visualization_msgs::InteractiveMarkerControl box_control; + box_control.always_visible = 1; + box_control.markers.push_back( box_marker ); + + // add the control to the interactive marker + i_marker.controls.push_back( box_control ); + + // create a control which will move the box, rviz will insert 2 arrows + visualization_msgs::InteractiveMarkerControl move_control; + move_control.name = "move_x"; + move_control.orientation.w = sqrt(2) / 2; + move_control.orientation.x = 0; + move_control.orientation.y = sqrt(2) / 2; + move_control.orientation.z = 0; + move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + + + // add the control to the interactive marker + i_marker.controls.push_back(move_control); + + // add the interactive marker to our collection + marker_server->insert(i_marker); + marker_server->setCallback(i_marker.name, feedback_cb); +} + +void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + std::stringstream ss(feedback->marker_name); + unsigned int index; + ss >> index; + if (index>=no_fixed_obstacles) + return; + PointObstacle* pobst = dynamic_cast(obst_vector.at(index).get()); + pobst->Position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y); +} \ No newline at end of file diff --git a/roborts_planning/local_planner/src/vel_converter.cpp b/roborts_planning/local_planner/src/vel_converter.cpp new file mode 100644 index 00000000..71fde447 --- /dev/null +++ b/roborts_planning/local_planner/src/vel_converter.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include +#include +#include + +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include "roborts_msgs/TwistAccel.h" + + +class VelConverter { + public: + VelConverter() : new_cmd_acc_(false), begin_(false) { + cmd_vel_.linear.x = 0; + cmd_vel_.linear.y = 0; + cmd_vel_.angular.z = 0; + + cmd_pub_ = cmd_handle_.advertise("/cmd_vel", 5); + cmd_sub_ = cmd_handle_.subscribe("/cmd_vel_acc", 100, boost::bind(&VelConverter::VelCallback, this, _1)); + } + void VelCallback(const roborts_msgs::TwistAccel::ConstPtr& msg); + void UpdateVel(); + + private: + roborts_msgs::TwistAccel cmd_vel_acc_; + geometry_msgs::Twist cmd_vel_; + + bool new_cmd_acc_, begin_; + + ros::NodeHandle cmd_handle_; + ros::Publisher cmd_pub_; + ros::Subscriber cmd_sub_; + std::chrono::high_resolution_clock::time_point time_begin_; + + std::mutex cmd_mutex_; +}; + +void VelConverter::VelCallback(const roborts_msgs::TwistAccel::ConstPtr& twist_acc_msg) +{ + if (!begin_) { + begin_ = true; + } + std::lock_guard cmd_guard(cmd_mutex_); + new_cmd_acc_ = true; + cmd_vel_acc_ = *twist_acc_msg; +} + +void VelConverter::UpdateVel() { + if (!begin_) { + return; + } + auto begin = std::chrono::high_resolution_clock::now(); + if (new_cmd_acc_) { + std::lock_guard cmd_guard(cmd_mutex_); + cmd_vel_ = cmd_vel_acc_.twist; + cmd_pub_.publish(cmd_vel_); + new_cmd_acc_ = false; + time_begin_ = std::chrono::high_resolution_clock::now(); + return; + } + auto actual_time = std::chrono::duration>(std::chrono::high_resolution_clock::now() - time_begin_).count(); + time_begin_ = std::chrono::high_resolution_clock::now(); + + cmd_vel_.linear.x = cmd_vel_.linear.x + actual_time * cmd_vel_acc_.accel.linear.x; + cmd_vel_.linear.y = cmd_vel_.linear.y + actual_time * cmd_vel_acc_.accel.linear.y; + cmd_vel_.angular.z = cmd_vel_.angular.z + actual_time * cmd_vel_acc_.accel.angular.z; + + cmd_pub_.publish(cmd_vel_); + +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "vel_converter"); + + VelConverter vel_converter; + + while (ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + ros::spinOnce(); + vel_converter.UpdateVel(); + } + + return 0; +} \ No newline at end of file diff --git a/roborts_planning/local_planner/timed_elastic_band/CMakeLists.txt b/roborts_planning/local_planner/timed_elastic_band/CMakeLists.txt new file mode 100644 index 00000000..bbc002ed --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.1) +project(timed_elastic_band) + + +aux_source_directory(src/. SRC_LIST) + +file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto/*.proto") +rrts_protobuf_generate_cpp( + ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto + timed_elastic_bandProtoSrc + timed_elastic_bandProtoHds + ${ProtoFiles}) + +add_library(${PROJECT_NAME} + SHARED + ${timed_elastic_bandProtoSrc} + ${timed_elastic_bandProtoHds} + ${SRC_LIST} + ) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + ${EXTERNAL_LIBS} + ) +add_library(planning::timed_elastic_band ALIAS timed_elastic_band) diff --git a/roborts_planning/local_planner/timed_elastic_band/config/hcp.prototxt b/roborts_planning/local_planner/timed_elastic_band/config/hcp.prototxt new file mode 100644 index 00000000..a0d75334 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/config/hcp.prototxt @@ -0,0 +1,21 @@ +hcp_opt { + enable_homotopy_class_planning: True + enable_multithreading: True + simple_exploration: False + max_number_classes: 4 + selection_obst_cost_scale: 1.0 + selection_prefer_initial_plan: 0.5 + selection_viapoint_cost_scale: 1.0 + selection_cost_hysteresis: 0.9 + selection_alternative_time_cost: False + roadmap_graph_no_samples: 15 + roadmap_graph_area_width: 5 + roadmap_graph_area_length_scale: 1.2 + h_signature_prescaler: 0.5 + h_signature_threshold: 0.1 + obstacle_keypoint_offset: 0.1 + obstacle_heading_threshold: 0.45 + viapoints_all_candidates: True + visualize_hc_graph: False +} + diff --git a/roborts_planning/local_planner/timed_elastic_band/config/recovery.prototxt b/roborts_planning/local_planner/timed_elastic_band/config/recovery.prototxt new file mode 100644 index 00000000..e3f80625 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/config/recovery.prototxt @@ -0,0 +1,10 @@ +recovery_info { + shrink_horizon_backup: True + shrink_horizon_min_duration: 10 + oscillation_recovery: true + oscillation_v_eps: 0.1 + oscillation_omega_eps: 0.1 + oscillation_recovery_min_duration: 10 + oscillation_filter_duration: 10 +} + diff --git a/roborts_planning/local_planner/timed_elastic_band/config/timed_elastic_band.prototxt b/roborts_planning/local_planner/timed_elastic_band/config/timed_elastic_band.prototxt new file mode 100644 index 00000000..9d7f80e4 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/config/timed_elastic_band.prototxt @@ -0,0 +1,96 @@ +opt_frame { + odom_frame: "odom" + map_frame: "map" +} + +trajectory_opt { + teb_autosize: True + dt_ref: 0.15 + dt_hysteresis: 0.01 + global_plan_overwrite_orientation: true + allow_init_with_backwards_motion: false + global_plan_viapoint_sep: 1.5 + via_points_ordered: False + max_global_plan_lookahead_dist: 2.0 + exact_arc_length: False + force_reinit_new_goal_dist: 0.8 + feasibility_check_no_poses: 5 + publish_feedback: False + min_samples: 3 + max_samples: 200 +} + +kinematics_opt { + max_vel_x: 2.5 + max_vel_x_backwards: 0.2 + max_vel_y: 2.5 + max_vel_theta: 2.5 + acc_lim_x: 2.6 + acc_lim_y: 2.6 + acc_lim_theta: 2.6 + min_turning_radius: 0 + wheelbase: 0 + cmd_angle_instead_rotvel: false +} + +tolerance_opt { + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.1 + free_goal_vel: False +} + +obstacles_opt { + min_obstacle_dist: 0.32 #0.8 + inflation_dist: 0 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 0.1 + obstacle_poses_affected: 30 + legacy_obstacle_association: False + obstacle_association_cutoff_factor: 5.0 + obstacle_association_force_inclusion_factor: 1.5 +} + +robot_type { + type: POINT + robot_vertices { + x: 0.3 + y: 0.225 + } + robot_vertices { + x: -0.3 + y: 0.225 + } + robot_vertices { + x: -0.3 + y: -0.225 + } + robot_vertices { + x: 0.3 + y: -0.225 + } +} + +optimize_info { + no_inner_iterations: 5 + no_outer_iterations: 4 + optimization_activate: True + optimization_verbose: False + penalty_epsilon: 0.1 + weight_max_vel_x: 1 + weight_max_vel_y: 1 + weight_max_vel_theta: 3 + weight_acc_lim_x: 1 + weight_acc_lim_y: 1 + weight_acc_lim_theta: 3 + weight_kinematics_nh: 1 + weight_kinematics_forward_drive: 1 + weight_kinematics_turning_radius: 0.0 + weight_optimaltime: 1 + weight_obstacle: 50 + weight_inflation: 0.1 + weight_dynamic_obstacle: 10 + weight_viapoint: 10 + weight_adapt_factor: 2.0 + weight_prefer_rotdir: 0 +} + diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/proto/timed_elastic_band.proto b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/proto/timed_elastic_band.proto new file mode 100644 index 00000000..5288f597 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/proto/timed_elastic_band.proto @@ -0,0 +1,158 @@ +syntax = "proto2"; + +package roborts_local_planner; +message FrameID { + optional string odom_frame = 1; + optional string map_frame = 2; +} + +message Point2D { + optional float x = 1; + optional float y = 2; +} + +message Trajectory { + optional bool teb_autosize = 1; + optional float dt_ref = 2; + optional float dt_hysteresis = 3; + optional bool global_plan_overwrite_orientation = 4; + optional bool allow_init_with_backwards_motion = 5; + optional float global_plan_viapoint_sep = 6; + optional bool via_points_ordered = 7; + optional float max_global_plan_lookahead_dist = 8; + optional bool exact_arc_length = 9; + optional float force_reinit_new_goal_dist = 10; + optional int64 feasibility_check_no_poses = 11; + optional bool publish_feedback = 12; + optional int64 min_samples = 13; + optional int64 max_samples = 14; +} + +message FootprintModel { + + enum FootprintType { + POINT = 0; + CIRCULAR = 1; + TWOCIRCLES = 2; + LINE = 3; + POLYGON = 4; + } + + optional FootprintType type = 1; + optional double radius = 2; + optional double front_offset = 3; + optional double front_radius = 4; + optional double rear_offset = 5; + optional double rear_radius = 6; + repeated Point2D robot_vertices = 7; + } + +message Robot { + optional float max_vel_x = 1; + optional float max_vel_x_backwards = 2; + optional float max_vel_y = 3; + optional float max_vel_theta = 4; + optional float acc_lim_x = 5; + optional float acc_lim_y = 6; + optional float acc_lim_theta = 7; + optional float min_turning_radius = 8; + optional float wheelbase = 9; + optional bool cmd_angle_instead_rotvel = 10; +} + + + +message GoalTolerance { + optional float xy_goal_tolerance = 1; + optional float yaw_goal_tolerance = 2; + optional bool free_goal_vel = 3; +} + + + +message Obstacles { + optional float min_obstacle_dist = 1; + optional float inflation_dist = 2; + optional bool include_costmap_obstacles = 3; + optional float costmap_obstacles_behind_robot_dist = 4; + optional float obstacle_poses_affected = 5; + optional bool legacy_obstacle_association = 6; + optional float obstacle_association_cutoff_factor = 7; + optional float obstacle_association_force_inclusion_factor = 8; + optional string costmap_converter_plugin = 9; + optional bool costmap_converter_spin_thread = 10; + optional float costmap_converter_rate = 11; +} + + + +message Optimization { + optional int32 no_inner_iterations = 1; + optional int32 no_outer_iterations = 2; + optional bool optimization_activate = 3; + optional bool optimization_verbose = 4; + optional float penalty_epsilon = 5; + optional float weight_max_vel_x = 6; + optional float weight_max_vel_y = 7; + optional float weight_max_vel_theta = 8; + optional float weight_acc_lim_x = 9; + optional float weight_acc_lim_y = 10; + optional float weight_acc_lim_thet = 11; + optional float weight_kinematics_nh = 12; + optional float weight_kinematics_forward_drive = 13; + optional float weight_kinematics_turning_radius = 14; + optional float weight_optimaltime = 15; + optional float weight_obstacle = 16; + optional float weight_inflation = 17; + optional float weight_dynamic_obstacle = 18; + optional float weight_viapoint = 19; + optional float weight_adapt_factor = 20; + optional float weight_prefer_rotdir = 21; + optional float weight_acc_lim_theta = 22; +} + + + +message HomotopyClassPlanner { + optional bool enable_homotopy_class_planning = 1; + optional bool enable_multithreading = 2; + optional bool simple_exploration = 3; + optional int32 max_number_classes = 4; + optional float selection_obst_cost_scale = 5; + optional float selection_prefer_initial_plan = 6; + optional float selection_viapoint_cost_scale = 7; + optional float selection_cost_hysteresis = 8; + optional bool selection_alternative_time_cost = 9; + optional int32 roadmap_graph_no_samples = 10; + optional int32 roadmap_graph_area_width = 11; + optional float roadmap_graph_area_length_scale = 12; + optional float h_signature_prescaler = 13; + optional float h_signature_threshold = 14; + optional float obstacle_keypoint_offset = 15; + optional float obstacle_heading_threshold = 16; + optional bool viapoints_all_candidates = 17; + optional bool visualize_hc_graph = 18; +} + +message Recovery { + optional bool shrink_horizon_min_duration = 1; + optional bool oscillation_recovery = 2; + optional bool shrink_horizon_backup = 3; + optional double oscillation_v_eps = 4; + optional double oscillation_omega_eps = 5; + optional double oscillation_recovery_min_duration = 6; + optional double oscillation_filter_duration = 7; +} + +message Config { + optional FrameID opt_frame = 1; + optional Trajectory trajectory_opt = 2; + optional Robot kinematics_opt = 3; + optional GoalTolerance tolerance_opt = 4; + optional Obstacles obstacles_opt = 5; + optional Optimization optimize_info = 6; + optional HomotopyClassPlanner hcp_opt = 7; + repeated FootprintModel robot_type = 8; + optional Recovery recovery_info = 9; +} + diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_acceleration_eage.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_acceleration_eage.h new file mode 100644 index 00000000..468e1b82 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_acceleration_eage.h @@ -0,0 +1,388 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_ACCELERATION_EAGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_ACCELERATION_EAGE_H + +#include + +#include "local_planner/utility_tool.h" + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_vertex_timediff.h" +#include "timed_elastic_band/teb_penalties.h" +#include "timed_elastic_band/teb_base_eage.h" + +namespace roborts_local_planner { + +class AccelerationEdge : public TebMultiEdgeBase<2, double> { + public: + + AccelerationEdge() { + this->resize(5); + } + + void computeError() { + const TebVertexPose *pose1 = static_cast(_vertices[0]); + const TebVertexPose *pose2 = static_cast(_vertices[1]); + const TebVertexPose *pose3 = static_cast(_vertices[2]); + const TebVertexTimeDiff *dt1 = static_cast(_vertices[3]); + const TebVertexTimeDiff *dt2 = static_cast(_vertices[4]); + + const Eigen::Vector2d diff1 = pose2->GetPose().GetPosition() - pose1->GetPose().GetPosition(); + const Eigen::Vector2d diff2 = pose3->GetPose().GetPosition() - pose2->GetPose().GetPosition(); + + double dist1 = diff1.norm(); + double dist2 = diff2.norm(); + const double angle_diff1 = g2o::normalize_theta(pose2->GetPose().GetTheta() - pose1->GetPose().GetTheta()); + const double angle_diff2 = g2o::normalize_theta(pose3->GetPose().GetTheta() - pose2->GetPose().GetTheta()); + + if (config_param_->trajectory_opt().exact_arc_length()) { + if (angle_diff1 != 0) { + const double radius = dist1 / (2 * sin(angle_diff1 / 2)); + dist1 = fabs(angle_diff1 * radius); + } + if (angle_diff2 != 0) { + const double radius = dist2 / (2 * sin(angle_diff2 / 2)); + dist2 = fabs(angle_diff2 * radius); + } + } + + double vel1 = dist1 / dt1->GetDiffTime(); + double vel2 = dist2 / dt2->GetDiffTime(); + + + vel1 *= LogisticSigmoid(100 * (diff1.x() * cos(pose1->GetPose().GetTheta()) + diff1.y() * sin(pose1->GetPose().GetTheta()))); + vel2 *= LogisticSigmoid(100 * (diff2.x() * cos(pose2->GetPose().GetTheta()) + diff2.y() * sin(pose2->GetPose().GetTheta()))); + + const double acc_lin = (vel2 - vel1) * 2 / (dt1->GetDiffTime() + dt2->GetDiffTime()); + + _error[0] = PenaltyBoundToInterval(acc_lin, config_param_->kinematics_opt().acc_lim_x(), + config_param_->optimize_info().penalty_epsilon()); + + const double omega1 = angle_diff1 / dt1->GetDiffTime(); + const double omega2 = angle_diff2 / dt2->GetDiffTime(); + const double acc_rot = (omega2 - omega1) * 2 / (dt1->GetDiffTime() + dt2->GetDiffTime()); + + _error[1] = PenaltyBoundToInterval(acc_rot, config_param_->kinematics_opt().acc_lim_theta(), + config_param_->optimize_info().penalty_epsilon()); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +class AccelerationStartEdge : public TebMultiEdgeBase<2, const geometry_msgs::Twist *> { + public: + + AccelerationStartEdge() { + _measurement = NULL; + this->resize(3); + } + + void computeError() { + const TebVertexPose *pose1 = static_cast(_vertices[0]); + const TebVertexPose *pose2 = static_cast(_vertices[1]); + const TebVertexTimeDiff *dt = static_cast(_vertices[2]); + + const Eigen::Vector2d diff = pose2->GetPose().GetPosition() - pose1->GetPose().GetPosition(); + double dist = diff.norm(); + const double angle_diff = g2o::normalize_theta(pose2->GetPose().GetTheta() - pose1->GetPose().GetTheta()); + if (config_param_->trajectory_opt().exact_arc_length() && angle_diff != 0) { + const double radius = dist / (2 * sin(angle_diff / 2)); + dist = fabs(angle_diff * radius); + } + + const double vel1 = _measurement->linear.x; + double vel2 = dist / dt->GetDiffTime(); + + vel2 *= LogisticSigmoid(100 * (diff.x() * cos(pose1->GetPose().GetTheta()) + diff.y() * sin(pose1->GetPose().GetTheta()))); + + const double acc_lin = (vel2 - vel1) / dt->GetDiffTime(); + + _error[0] = PenaltyBoundToInterval(acc_lin, config_param_->kinematics_opt().acc_lim_x(), + config_param_->optimize_info().penalty_epsilon()); + + const double omega1 = _measurement->angular.z; + const double omega2 = angle_diff / dt->GetDiffTime(); + const double acc_rot = (omega2 - omega1) / dt->GetDiffTime(); + + _error[1] = PenaltyBoundToInterval(acc_rot, config_param_->kinematics_opt().acc_lim_theta(), + config_param_->optimize_info().penalty_epsilon()); + } + + void SetInitialVelocity(const geometry_msgs::Twist &vel_start) { + _measurement = &vel_start; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class AccelerationGoalEdge : public TebMultiEdgeBase<2, const geometry_msgs::Twist *> { + public: + + AccelerationGoalEdge() { + _measurement = NULL; + this->resize(3); + } + + void computeError() { + const TebVertexPose *pose_pre_goal = static_cast(_vertices[0]); + const TebVertexPose *pose_goal = static_cast(_vertices[1]); + const TebVertexTimeDiff *dt = static_cast(_vertices[2]); + + const Eigen::Vector2d diff = pose_goal->GetPose().GetPosition() - pose_pre_goal->GetPose().GetPosition(); + double dist = diff.norm(); + const double angle_diff = g2o::normalize_theta(pose_goal->GetPose().GetTheta() - pose_pre_goal->GetPose().GetTheta()); + if (config_param_->trajectory_opt().exact_arc_length() && angle_diff != 0) { + double radius = dist / (2 * sin(angle_diff / 2)); + dist = fabs(angle_diff * radius); + } + + double vel1 = dist / dt->GetDiffTime(); + const double vel2 = _measurement->linear.x; + + vel1 *= LogisticSigmoid(100 * (diff.x() * cos(pose_pre_goal->GetPose().GetTheta()) + diff.y() * sin(pose_pre_goal->GetPose().GetTheta()))); + + const double acc_lin = (vel2 - vel1) / dt->GetDiffTime(); + + _error[0] = PenaltyBoundToInterval(acc_lin, config_param_->kinematics_opt().acc_lim_x(), + config_param_->optimize_info().penalty_epsilon()); + + const double omega1 = angle_diff / dt->GetDiffTime(); + const double omega2 = _measurement->angular.z; + const double acc_rot = (omega2 - omega1) / dt->GetDiffTime(); + + _error[1] = PenaltyBoundToInterval(acc_rot, config_param_->kinematics_opt().acc_lim_theta(), + config_param_->optimize_info().penalty_epsilon()); + + } + + void SetGoalVelocity(const geometry_msgs::Twist &vel_goal) { + _measurement = &vel_goal; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class AccelerationHolonomicEdge : public TebMultiEdgeBase<3, double> { + public: + + AccelerationHolonomicEdge() { + this->resize(5); + } + + void computeError() { + const TebVertexPose *pose1 = static_cast(_vertices[0]); + const TebVertexPose *pose2 = static_cast(_vertices[1]); + const TebVertexPose *pose3 = static_cast(_vertices[2]); + const TebVertexTimeDiff *dt1 = static_cast(_vertices[3]); + const TebVertexTimeDiff *dt2 = static_cast(_vertices[4]); + + Eigen::Vector2d diff1 = pose2->GetPose().GetPosition() - pose1->GetPose().GetPosition(); + Eigen::Vector2d diff2 = pose3->GetPose().GetPosition() - pose2->GetPose().GetPosition(); + + double cos_theta1 = std::cos(pose1->GetPose().GetTheta()); + double sin_theta1 = std::sin(pose1->GetPose().GetTheta()); + double cos_theta2 = std::cos(pose2->GetPose().GetTheta()); + double sin_theta2 = std::sin(pose2->GetPose().GetTheta()); + + double p1_dx = cos_theta1 * diff1.x() + sin_theta1 * diff1.y(); + double p1_dy = -sin_theta1 * diff1.x() + cos_theta1 * diff1.y(); + + double p2_dx = cos_theta2 * diff2.x() + sin_theta2 * diff2.y(); + double p2_dy = -sin_theta2 * diff2.x() + cos_theta2 * diff2.y(); + + double vel1_x = p1_dx / dt1->GetDiffTime(); + double vel1_y = p1_dy / dt1->GetDiffTime(); + double vel2_x = p2_dx / dt2->GetDiffTime(); + double vel2_y = p2_dy / dt2->GetDiffTime(); + + double dt12 = dt1->GetDiffTime() + dt2->GetDiffTime(); + + double acc_x = (vel2_x - vel1_x) * 2 / dt12; + double acc_y = (vel2_y - vel1_y) * 2 / dt12; + + _error[0] = PenaltyBoundToInterval(acc_x, config_param_->kinematics_opt().acc_lim_x(), + config_param_->optimize_info().penalty_epsilon()); + _error[1] = PenaltyBoundToInterval(acc_y, config_param_->kinematics_opt().acc_lim_y(), + config_param_->optimize_info().penalty_epsilon()); + + double omega1 = g2o::normalize_theta(pose2->GetPose().GetTheta() - pose1->GetPose().GetTheta()) / dt1->GetDiffTime(); + double omega2 = g2o::normalize_theta(pose3->GetPose().GetTheta() - pose2->GetPose().GetTheta()) / dt2->GetDiffTime(); + double acc_rot = (omega2 - omega1) * 2 / dt12; + + _error[2] = PenaltyBoundToInterval(acc_rot, config_param_->kinematics_opt().acc_lim_theta(), + config_param_->optimize_info().penalty_epsilon()); + + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +class AccelerationHolonomicStartEdge : public TebMultiEdgeBase<3, const geometry_msgs::Twist *> { + public: + + AccelerationHolonomicStartEdge() { + this->resize(3); + _measurement = NULL; + } + + void computeError() { + const TebVertexPose *pose1 = static_cast(_vertices[0]); + const TebVertexPose *pose2 = static_cast(_vertices[1]); + const TebVertexTimeDiff *dt = static_cast(_vertices[2]); + + Eigen::Vector2d diff = pose2->GetPose().GetPosition() - pose1->GetPose().GetPosition(); + + double cos_theta1 = std::cos(pose1->GetPose().GetTheta()); + double sin_theta1 = std::sin(pose1->GetPose().GetTheta()); + + double p1_dx = cos_theta1 * diff.x() + sin_theta1 * diff.y(); + double p1_dy = -sin_theta1 * diff.x() + cos_theta1 * diff.y(); + + double vel1_x = _measurement->linear.x; + double vel1_y = _measurement->linear.y; + double vel2_x = p1_dx / dt->GetDiffTime(); + double vel2_y = p1_dy / dt->GetDiffTime(); + + double acc_lin_x = (vel2_x - vel1_x) / dt->GetDiffTime(); + double acc_lin_y = (vel2_y - vel1_y) / dt->GetDiffTime(); + + _error[0] = PenaltyBoundToInterval(acc_lin_x, config_param_->kinematics_opt().acc_lim_x(), + config_param_->optimize_info().penalty_epsilon()); + _error[1] = PenaltyBoundToInterval(acc_lin_y, config_param_->kinematics_opt().acc_lim_y(), + config_param_->optimize_info().penalty_epsilon()); + + double omega1 = _measurement->angular.z; + double omega2 = g2o::normalize_theta(pose2->GetPose().GetTheta() - pose1->GetPose().GetTheta()) / dt->GetDiffTime(); + double acc_rot = (omega2 - omega1) / dt->GetDiffTime(); + + _error[2] = PenaltyBoundToInterval(acc_rot, config_param_->kinematics_opt().acc_lim_theta(), + config_param_->optimize_info().penalty_epsilon()); + + } + + void setInitialVelocity(const geometry_msgs::Twist &vel_start) { + _measurement = &vel_start; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class AccelerationHolonomicGoalEdge : public TebMultiEdgeBase<3, const geometry_msgs::Twist *> { + public: + + AccelerationHolonomicGoalEdge() { + _measurement = NULL; + this->resize(3); + } + + void computeError() { + + const TebVertexPose *pose_pre_goal = static_cast(_vertices[0]); + const TebVertexPose *pose_goal = static_cast(_vertices[1]); + const TebVertexTimeDiff *dt = static_cast(_vertices[2]); + + Eigen::Vector2d diff = pose_goal->GetPose().GetPosition() - pose_pre_goal->GetPose().GetPosition(); + + double cos_theta1 = std::cos(pose_pre_goal->GetPose().GetTheta()); + double sin_theta1 = std::sin(pose_pre_goal->GetPose().GetTheta()); + + double p1_dx = cos_theta1 * diff.x() + sin_theta1 * diff.y(); + double p1_dy = -sin_theta1 * diff.x() + cos_theta1 * diff.y(); + + double vel1_x = p1_dx / dt->GetDiffTime(); + double vel1_y = p1_dy / dt->GetDiffTime(); + double vel2_x = _measurement->linear.x; + double vel2_y = _measurement->linear.y; + + double acc_lin_x = (vel2_x - vel1_x) / dt->GetDiffTime(); + double acc_lin_y = (vel2_y - vel1_y) / dt->GetDiffTime(); + + _error[0] = PenaltyBoundToInterval(acc_lin_x, config_param_->kinematics_opt().acc_lim_x(), + config_param_->optimize_info().penalty_epsilon()); + _error[1] = PenaltyBoundToInterval(acc_lin_x, config_param_->kinematics_opt().acc_lim_y(), + config_param_->optimize_info().penalty_epsilon()); + + double omega1 = g2o::normalize_theta(pose_goal->GetPose().GetTheta() - pose_pre_goal->GetPose().GetTheta()) / dt->GetDiffTime(); + double omega2 = _measurement->angular.z; + double acc_rot = (omega2 - omega1) / dt->GetDiffTime(); + + _error[2] = PenaltyBoundToInterval(acc_rot, config_param_->kinematics_opt().acc_lim_theta(), + config_param_->optimize_info().penalty_epsilon()); + } + + void SetGoalVelocity(const geometry_msgs::Twist &vel_goal) { + _measurement = &vel_goal; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_ACCELERATION_EAGE_H_ diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_base_eage.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_base_eage.h new file mode 100644 index 00000000..db710174 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_base_eage.h @@ -0,0 +1,213 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_BASE_EAGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_BASE_EAGE_H + +#include + +#include +#include +#include + +#include "timed_elastic_band/proto/timed_elastic_band.pb.h" + +namespace roborts_local_planner { +template +class TebUnaryEdgeBase : public g2o::BaseUnaryEdge { + public: + using typename g2o::BaseUnaryEdge::ErrorVector; + using g2o::BaseUnaryEdge::computeError; + + TebUnaryEdgeBase () { + _vertices[0] = NULL; + } + + virtual ~TebUnaryEdgeBase() { + if (_vertices[0]) { + _vertices[0]->edges().erase(this); + } + } + + ErrorVector& GetError () { + computeError(); + return _error; + } + void SetConfig(const Config &config_param) { + config_param_ = &config_param; + } + virtual bool read(std::istream& is) { + return true; + } + virtual bool write(std::ostream& os) const { + return os.good(); + } + + + protected: + + using g2o::BaseUnaryEdge::_error; + using g2o::BaseUnaryEdge::_vertices; + const Config *config_param_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; // class TebBaseEage + +template +class TebBinaryEdgeBase : public g2o::BaseBinaryEdge { + public: + + using typename g2o::BaseBinaryEdge::ErrorVector; + using g2o::BaseBinaryEdge::computeError; + + TebBinaryEdgeBase() { + _vertices[0] = _vertices[1] = NULL; + } + + virtual ~TebBinaryEdgeBase() { + if(_vertices[0]) + _vertices[0]->edges().erase(this); + if(_vertices[1]) + _vertices[1]->edges().erase(this); + } + + ErrorVector& GetError() { + computeError(); + return _error; + } + + void SetConfig(const Config &config_param) { + config_param_ = &config_param; + } + + virtual bool read(std::istream& is) { + return true; + } + + virtual bool write(std::ostream& os) const { + return os.good(); + } + + protected: + + using g2o::BaseBinaryEdge::_error; + using g2o::BaseBinaryEdge::_vertices; + const Config *config_param_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +template +class TebMultiEdgeBase : public g2o::BaseMultiEdge { + + public: + using typename g2o::BaseMultiEdge::ErrorVector; + using g2o::BaseMultiEdge::computeError; + + TebMultiEdgeBase() { + + } + + virtual ~TebMultiEdgeBase() { + for(std::size_t i=0; i<_vertices.size(); ++i) { + if(_vertices[i]) + _vertices[i]->edges().erase(this); + } + } + + virtual void resize(size_t size) { + g2o::BaseMultiEdge::resize(size); + + for(std::size_t i=0; i<_vertices.size(); ++i) + _vertices[i] = NULL; + } + + ErrorVector& GetError() { + computeError(); + return _error; + } + + void SetConfig(const Config &config_param) { + config_param_ = &config_param; + } + + virtual bool read(std::istream& is) { + return true; + } + + virtual bool write(std::ostream& os) const { + return os.good(); + } + + protected: + + using g2o::BaseMultiEdge::_error; + using g2o::BaseMultiEdge::_vertices; + const Config *config_param_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace roborts_local_planner + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_BASE_EAGE_H \ No newline at end of file diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_kinematics_edge.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_kinematics_edge.h new file mode 100644 index 00000000..c412b209 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_kinematics_edge.h @@ -0,0 +1,173 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_KINEMATICS_EDGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_KINEMATICS_EDGE_H + +#include + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_penalties.h" +#include "timed_elastic_band/teb_base_eage.h" + +#define USE_ANALYTIC_JACOBI + +namespace roborts_local_planner { + +class KinematicsDiffDriveEdge : public TebBinaryEdgeBase<2, double, TebVertexPose, TebVertexPose> { + public: + KinematicsDiffDriveEdge() { + this->setMeasurement(0.); + } + + void computeError() { + const TebVertexPose *conf1 = static_cast(_vertices[0]); + const TebVertexPose *conf2 = static_cast(_vertices[1]); + + Eigen::Vector2d deltaS = conf2->GetPose().GetPosition() - conf1->GetPose().GetPosition(); + + _error[0] = fabs((cos(conf1->GetPose().GetTheta()) + cos(conf2->GetPose().GetTheta())) * deltaS[1] + - (sin(conf1->GetPose().GetTheta()) + sin(conf2->GetPose().GetTheta())) * deltaS[0]); + + Eigen::Vector2d angle_vec(cos(conf1->GetPose().GetTheta()), sin(conf1->GetPose().GetTheta())); + _error[1] = PenaltyBoundFromBelow(deltaS.dot(angle_vec), 0, 0); + + } + +#ifdef USE_ANALYTIC_JACOBI +#if 1 + + void linearizeOplus() { + const TebVertexPose *conf1 = static_cast(_vertices[0]); + const TebVertexPose *conf2 = static_cast(_vertices[1]); + + Eigen::Vector2d deltaS = conf2->GetPose().GetPosition() - conf1->GetPose().GetPosition(); + + double cos1 = cos(conf1->GetPose().GetTheta()); + double cos2 = cos(conf2->GetPose().GetTheta()); + double sin1 = sin(conf1->GetPose().GetTheta()); + double sin2 = sin(conf2->GetPose().GetTheta()); + double aux1 = sin1 + sin2; + double aux2 = cos1 + cos2; + + double dd_error_1 = deltaS[0] * cos1; + double dd_error_2 = deltaS[1] * sin1; + double dd_dev = PenaltyBoundFromBelowDerivative(dd_error_1 + dd_error_2, 0, 0); + + double dev_nh_abs = g2o::sign((cos(conf1->GetPose().GetTheta()) + cos(conf2->GetPose().GetTheta())) * deltaS[1] - + (sin(conf1->GetPose().GetTheta()) + sin(conf2->GetPose().GetTheta())) * deltaS[0]); + + _jacobianOplusXi(0, 0) = aux1 * dev_nh_abs; + _jacobianOplusXi(0, 1) = -aux2 * dev_nh_abs; + _jacobianOplusXi(1, 0) = -cos1 * dd_dev; + _jacobianOplusXi(1, 1) = -sin1 * dd_dev; + _jacobianOplusXi(0, 2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; + _jacobianOplusXi(1, 2) = (-sin1 * deltaS[0] + cos1 * deltaS[1]) * dd_dev; + + + _jacobianOplusXj(0, 0) = -aux1 * dev_nh_abs; + _jacobianOplusXj(0, 1) = aux2 * dev_nh_abs; + _jacobianOplusXj(1, 0) = cos1 * dd_dev; + _jacobianOplusXj(1, 1) = sin1 * dd_dev; + _jacobianOplusXj(0, 2) = (-sin2 * deltaS[1] - cos2 * deltaS[0]) * dev_nh_abs; + _jacobianOplusXj(1, 2) = 0; + } +#endif +#endif + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class KinematicsCarlikeEdge : public TebBinaryEdgeBase<2, double, TebVertexPose, TebVertexPose> { + public: + + KinematicsCarlikeEdge() { + this->setMeasurement(0.); + } + + void computeError() { + const TebVertexPose *conf1 = static_cast(_vertices[0]); + const TebVertexPose *conf2 = static_cast(_vertices[1]); + + Eigen::Vector2d deltaS = conf2->GetPose().GetPosition() - conf1->GetPose().GetPosition(); + + _error[0] = fabs((cos(conf1->GetPose().GetTheta()) + cos(conf2->GetPose().GetTheta())) * deltaS[1] + - (sin(conf1->GetPose().GetTheta()) + sin(conf2->GetPose().GetTheta())) * deltaS[0]); + + double angle_diff = g2o::normalize_theta(conf2->GetPose().GetTheta() - conf1->GetPose().GetTheta()); + if (angle_diff == 0) { + _error[1] = 0; + } else if (config_param_->trajectory_opt().exact_arc_length()) { + _error[1] = PenaltyBoundFromBelow(fabs(deltaS.norm() / (2 * sin(angle_diff / 2))), + config_param_->kinematics_opt().min_turning_radius(), 0.0); + } else { + _error[1] = PenaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), + config_param_->kinematics_opt().min_turning_radius(), 0.0); + } + + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_KINEMATICS_EDGE_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_local_planner.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_local_planner.h new file mode 100644 index 00000000..0b52569e --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_local_planner.h @@ -0,0 +1,208 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_H + +#include + +#include "io/io.h" +#include "state/error_code.h" +#include "alg_factory/algorithm_factory.h" + +#include "costmap/costmap_interface.h" + +#include "local_planner/local_planner_base.h" +#include "local_planner/optimal_base.h" +#include "local_planner/robot_position_cost.h" +#include "local_planner/local_visualization.h" +#include "local_planner/utility_tool.h" +#include "local_planner/odom_info.h" +#include "local_planner/data_converter.h" +#include "local_planner/data_base.h" +#include "local_planner/obstacle.h" +#include "local_planner/robot_footprint_model.h" + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_optimal.h" +#include "timed_elastic_band/proto/timed_elastic_band.pb.h" + +namespace roborts_local_planner { + +/** + * @brief See local_planner_base.h + */ +class TebLocalPlanner : public LocalPlannerBase { + public: + TebLocalPlanner(); + ~TebLocalPlanner(); + roborts_common::ErrorInfo ComputeVelocityCommands(roborts_msgs::TwistAccel &cmd_vel) override; + bool IsGoalReached () override; + roborts_common::ErrorInfo Initialize (std::shared_ptr local_cost, + std::shared_ptr tf, LocalVisualizationPtr visual) override; + bool SetPlan(const nav_msgs::Path& plan, const geometry_msgs::PoseStamped& goal) override ; + bool GetPlan(const nav_msgs::Path& plan); + bool SetPlanOrientation(); + void RegisterErrorCallBack(ErrorInfoCallback error_callback) override; + + bool PruneGlobalPlan(); + + bool TransformGlobalPlan(int *current_goal_idx = NULL); + + double EstimateLocalGoalOrientation(const DataBase& local_goal, + int current_goal_idx, int moving_average_length=3) const; + + void UpdateViaPointsContainer(); + + void SaturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, + double max_vel_theta, double max_vel_x_backwards) const; + + void UpdateObstacleWithCostmap(Eigen::Vector2d local_goal); + void UpdateRobotPose(); + void UpdateRobotVel(); + void UpdateGlobalToPlanTranform(); + bool CutAndTransformGlobalPlan(int *current_goal_idx = NULL); + + double ConvertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const; + + //! Tf listener + std::weak_ptr tf_; + //! Local cost map + std::weak_ptr local_cost_; + + //! Local planner frame(local planner will do optimal in this frame), different with global planner frame + std::string global_frame_; + //! Local planner costmap 2d + roborts_costmap::Costmap2D *costmap_; + //! Robot footprint cost + std::shared_ptr robot_cost_; + //! Optimal based algorithm ptr + OptimalBasePtr optimal_; + //! Obstacle ptr + std::vector obst_vector_; + //! Must via point + ViaPointContainer via_points_; + //! Robot footprint + std::vector robot_footprint_; + //! Robot inscribed radius + double robot_inscribed_radius_; + //! Robot circumscribed radius + double robot_circumscribed_radius; + //! Robot odom info + OdomInfo odom_info_; + //! Global planner's solve + nav_msgs::Path global_plan_, temp_plan_; + //! Last velocity + roborts_msgs::TwistAccel last_cmd_; + //! Robot current velocity + geometry_msgs::Twist robot_current_vel_; + //! Robot current pose + DataBase robot_pose_; + //! Robot current pose + tf::Stamped robot_tf_pose_; + //! Robot goal + DataBase robot_goal_; + //! Visualize ptr use to visualize trajectory after optimize + LocalVisualizationPtr visual_; + //! Tf transform from global planner frame to optimal frame + tf::StampedTransform plan_to_global_transform_; + //! Way point after tf transform + std::vector transformed_plan_; + //! When no global planner give the global plan, use local goal express robot end point + tf::Stamped local_goal_; + //! Error info when running teb local planner algorithm + roborts_common::ErrorInfo teb_error_info_; + //! Call back function use to return error info + ErrorInfoCallback error_callback_; + //! Time begin when robot oscillation at a position + std::chrono::system_clock::time_point oscillation_; + //! Time allow robot oscillation at a position + double oscillation_time_; + //! Robot last position + DataBase last_robot_pose_; + //! Plan mutex + std::mutex plan_mutex_; + + //! Optimal param + Config param_config_; + bool free_goal_vel_; + bool global_plan_overwrite_orientation_; + float cut_lookahead_dist_; + long fesiable_step_look_ahead_; + float max_vel_x_; + float max_vel_y_; + float max_vel_theta_; + float max_vel_x_backwards; + float xy_goal_tolerance_; + float yaw_goal_tolerance_; + float osbtacle_behind_robot_dist_; + + + protected: + //! Check if the algorithm is initialized + bool is_initialized_ = false; +}; + +/** + * Register the algorithm to algorithm factory + */ +roborts_common::REGISTER_ALGORITHM(LocalPlannerBase, "timed_elastic_band", TebLocalPlanner); + +} // namespace roborts_local_planner + + + + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_H \ No newline at end of file diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_obstacle_eage.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_obstacle_eage.h new file mode 100644 index 00000000..46f6d400 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_obstacle_eage.h @@ -0,0 +1,155 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_OBSTACLE_EDGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_OBSTACLE_EDGE_H +#include "local_planner/obstacle.h" +#include "local_planner/robot_footprint_model.h" +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_base_eage.h" +#include "timed_elastic_band/teb_penalties.h" + + + +namespace roborts_local_planner { + +class ObstacleEdge : public TebUnaryEdgeBase<1, const Obstacle *, TebVertexPose> { + public: + + ObstacleEdge() { + _measurement = NULL; + } + + void computeError() { + const TebVertexPose *bandpt = static_cast(_vertices[0]); + + double dist = robot_model_->CalculateDistance(bandpt->GetPose(), _measurement); + + _error[0] = PenaltyBoundFromBelow(dist, config_param_->obstacles_opt().min_obstacle_dist(), + config_param_->optimize_info().penalty_epsilon()); + + } + + void SetObstacle(const Obstacle *obstacle) { + _measurement = obstacle; + } + void SetRobotModel(const BaseRobotFootprintModel *robot_model) { + robot_model_ = robot_model; + } + + void SetParameters(const Config &config_param,const BaseRobotFootprintModel *robot_model, const Obstacle *obstacle) { + config_param_ = &config_param; + robot_model_ = robot_model; + _measurement = obstacle; + } + + protected: + + const BaseRobotFootprintModel *robot_model_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +class InflatedObstacleEdge : public TebUnaryEdgeBase<2, const Obstacle *, TebVertexPose> { + public: + + InflatedObstacleEdge() { + _measurement = NULL; + } + + void computeError() { + const TebVertexPose *bandpt = static_cast(_vertices[0]); + + double dist = robot_model_->CalculateDistance(bandpt->GetPose(), _measurement); + + _error[0] = PenaltyBoundFromBelow(dist, config_param_->obstacles_opt().min_obstacle_dist(), + config_param_->optimize_info().penalty_epsilon()); + _error[1] = PenaltyBoundFromBelow(dist, config_param_->obstacles_opt().inflation_dist(), 0.0); + + } + + void SetObstacle(const Obstacle *obstacle) { + _measurement = obstacle; + } + + void SetRobotModel(const BaseRobotFootprintModel *robot_model) { + robot_model_ = robot_model; + } + + void SetParameters(const Config &config_param, const BaseRobotFootprintModel *robot_model, const Obstacle *obstacle) { + config_param_ = &config_param; + robot_model_ = robot_model; + _measurement = obstacle; + } + + protected: + + const BaseRobotFootprintModel *robot_model_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +} // namespace roborts_local_planner + +#endif diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_optimal.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_optimal.h new file mode 100644 index 00000000..0268455d --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_optimal.h @@ -0,0 +1,269 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_OPTIMAL_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_OPTIMAL_H +#include +#include + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "local_planner/robot_position_cost.h" +#include "local_planner/optimal_base.h" +#include "local_planner/obstacle.h" +#include "local_planner/robot_footprint_model.h" +#include "local_planner/utility_tool.h" +#include "local_planner/local_visualization.h" +#include "local_planner/robot_position_cost.h" +#include "local_planner/data_base.h" + +#include "timed_elastic_band/teb_vertex_console.h" +#include "timed_elastic_band/teb_acceleration_eage.h" +#include "timed_elastic_band/teb_kinematics_edge.h" +#include "timed_elastic_band/teb_obstacle_eage.h" +#include "timed_elastic_band/teb_prefer_rotdir_edge.h" +#include "timed_elastic_band/teb_time_optimal_eage.h" +#include "timed_elastic_band/teb_velocity_eage.h" +#include "timed_elastic_band/teb_via_point_edge.h" +#include "timed_elastic_band/proto/timed_elastic_band.pb.h" + + +namespace roborts_local_planner { + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TebBlockSolver; +typedef g2o::LinearSolverCSparse TebLinearSolver; +typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > ViaPointContainer; +/** + * @brief See optimal_base.h + */ +class TebOptimal : public OptimalBase { + public: + + TebOptimal(); + + TebOptimal (const Config& config_param, ObstContainer* obstacles = NULL, + RobotFootprintModelPtr robot_model = boost::make_shared(), + LocalVisualizationPtr visual = LocalVisualizationPtr(),const ViaPointContainer* via_points = NULL); + + ~TebOptimal() { + + } + + void initialize(const Config& config_param, ObstContainer* obstacles = NULL, + RobotFootprintModelPtr robot_model = boost::make_shared(), + LocalVisualizationPtr visual = LocalVisualizationPtr(),const ViaPointContainer* via_points = NULL); + + bool Optimal(std::vector& initial_plan, const geometry_msgs::Twist* start_vel = NULL, + bool free_goal_vel = false, bool micro_control = false) override; + + bool Optimal(const DataBase& start, const DataBase& goal, const geometry_msgs::Twist* start_vel = NULL, + bool free_goal_vel=false, bool micro_control = false) override; + + bool GetVelocity(roborts_common::ErrorInfo &error_info, double& vx, double& vy, double& omega, + double& acc_x, double& acc_y, double& acc_omega) const override ; + + bool OptimizeTeb(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false, + double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); + + void SetVisualization(LocalVisualizationPtr visualize); + + virtual void Visualize(); + + void SetVelocityStart(const geometry_msgs::Twist& vel_start); + + void SetVelocityEnd(const geometry_msgs::Twist& vel_end); + + + void SetVelocityGoalFree() { + vel_end_.first = false; + } + + void SetObstVector(ObstContainer* obst_vector) { + obstacles_ = obst_vector; + } + + const ObstContainer& GetObstVector() const { + return *obstacles_; + } + + void SetViaPoints(const ViaPointContainer* via_points) { + via_points_ = via_points; + } + + const ViaPointContainer& GetViaPoints() const { + return *via_points_; + } + + void ClearPlanner() override { + ClearGraph(); + } + + virtual void SetPreferredTurningDir(RotType dir) { + prefer_rotdir_=dir; + } + + static void RegisterG2OTypes(); + + boost::shared_ptr Optimizer() { + return optimizer_; + } + + bool IsOptimized() const { + return optimized_; + } + + void ComputeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); + + virtual void ComputeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, + double viapoint_cost_scale=1.0, bool alternative_time_cost=false) { + ComputeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); + cost.push_back( GetCurrentCost() ); + } + + double GetCurrentCost() const { + return cost_; + } + + inline void ExtractVelocity(const DataBase& pose1, const DataBase& pose2, double dt, + double& vx, double& vy, double& omega) const; + + void GetVelocityProfile(std::vector& velocity_profile) const; + + //void GetFullTrajectory(std::vector& trajectory) const; + + bool IsTrajectoryFeasible(roborts_common::ErrorInfo &error_info, RobotPositionCost* position_cost, const std::vector& footprint_spec, + double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1) override ; + + bool IsHorizonReductionAppropriate(const std::vector& initial_plan) const override ; + + protected: + bool BuildGraph(double weight_multiplier=1.0); + + bool OptimizeGraph(int no_iterations, bool clear_after=true); + + void ClearGraph(); + + void AddTebVertices(); + + void AddVelocityEdges(); + + void AddAccelerationEdges(); + + void AddTimeOptimalEdges(); + + void AddObstacleEdges(double weight_multiplier=1.0); + + void AddObstacleLegacyEdges(double weight_multiplier=1.0); + + void AddViaPointsEdges(); + + void AddPreferRotDirEdges(); + + void AddKinematicsDiffDriveEdges(); + + void AddKinematicsCarlikeEdges(); + + void AddDynamicObstaclesEdges(); + + + + boost::shared_ptr InitOptimizer(); + + ObstContainer* obstacles_; + const ViaPointContainer* via_points_; + double cost_; + RotType prefer_rotdir_; + LocalVisualizationPtr visualization_; + + + RobotFootprintModelPtr robot_model_; + TebVertexConsole vertex_console_; + boost::shared_ptr optimizer_; + std::pair vel_start_; + std::pair vel_end_; + + Robot robot_info_; + Config param_config_; + Obstacles obstacles_info_; + Trajectory trajectory_info_; + Optimization optimization_info_; + + bool initialized_; + bool optimized_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + +typedef boost::shared_ptr TebOptimalPlannerPtr; + +} // namespace roborts_local_planner + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_OPTIMAL_H \ No newline at end of file diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_penalties.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_penalties.h new file mode 100644 index 00000000..07043c32 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_penalties.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_PENALTIES_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_PENALTIES_H + +#include +#include +#include + +namespace roborts_local_planner { + +inline double PenaltyBoundToInterval(const double &var, const double &a, const double &epsilon) { + if (var < -a + epsilon) { + return (-var - (a - epsilon)); + } + if (var <= a - epsilon) { + return 0.; + } else { + return (var - (a - epsilon)); + } +} + +inline double PenaltyBoundToInterval(const double &var, const double &a, const double &b, const double &epsilon) { + if (var < a + epsilon) { + return (-var + (a + epsilon)); + } + if (var <= b - epsilon) { + return 0.; + } else { + return (var - (b - epsilon)); + } +} + +inline double PenaltyBoundFromBelow(const double &var, const double &a, const double &epsilon) { + if (var >= a + epsilon) { + return 0.; + } else { + return (-var + (a + epsilon)); + } +} + +inline double PenaltyBoundToIntervalDerivative(const double &var, const double &a, const double &epsilon) { + if (var < -a + epsilon) { + return -1; + } + if (var <= a - epsilon) { + return 0.; + } else { + return 1; + } +} + +inline double PenaltyBoundToIntervalDerivative(const double &var, + const double &a, + const double &b, + const double &epsilon) { + if (var < a + epsilon) { + return -1; + } + if (var <= b - epsilon) { + return 0.; + } else { + return 1; + } +} + +inline double PenaltyBoundFromBelowDerivative(const double &var, const double &a, const double &epsilon) { + if (var >= a + epsilon) { + return 0.; + } else { + return -1; + } +} + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_PENALTIES_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_prefer_rotdir_edge.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_prefer_rotdir_edge.h new file mode 100644 index 00000000..d7936ca9 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_prefer_rotdir_edge.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_PREFER_ROTDIR_EDGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_PREFER_ROTDIR_EDGE_H + +#include + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_base_eage.h" +#include "timed_elastic_band/teb_penalties.h" + +namespace roborts_local_planner { + + +class PreferRotDirEdge : public TebBinaryEdgeBase<1, double, TebVertexPose, TebVertexPose> { + public: + + PreferRotDirEdge() { + _measurement = 1; + } + + void computeError() { + const TebVertexPose *conf1 = static_cast(_vertices[0]); + const TebVertexPose *conf2 = static_cast(_vertices[1]); + + _error[0] = PenaltyBoundFromBelow(_measurement * g2o::normalize_theta(conf2->GetPose().GetTheta() - conf1->GetPose().GetTheta()), 0, 0); + } + + void SetRotDir(double dir) { + _measurement = dir; + } + + void PreferRight() { _measurement = -1; } + + void PreferLeft() { _measurement = 1; } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_PREFER_ROTDIR_EDGE_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_time_optimal_eage.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_time_optimal_eage.h new file mode 100644 index 00000000..a49db756 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_time_optimal_eage.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_TIME_OPTIMAL_EDGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_TIME_OPTIMAL_EDGE_H + +#include + +#include + +#include "timed_elastic_band/teb_vertex_timediff.h" +#include "timed_elastic_band/teb_base_eage.h" +#include "timed_elastic_band/teb_penalties.h" + +#define USE_ANALYTIC_JACOBI + +namespace roborts_local_planner { + +class TimeOptimalEdge : public TebUnaryEdgeBase<1, double, TebVertexTimeDiff> { + public: + TimeOptimalEdge() { + this->setMeasurement(0.); + } + + void computeError() { + const TebVertexTimeDiff *timediff = static_cast(_vertices[0]); + + _error[0] = timediff->GetDiffTime(); + + } + +#ifdef USE_ANALYTIC_JACOBI + + void linearizeOplus() { + _jacobianOplusXi(0, 0) = 1; + } +#endif + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_TIME_OPTIMAL_EDGE_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_velocity_eage.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_velocity_eage.h new file mode 100644 index 00000000..06f75fe2 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_velocity_eage.h @@ -0,0 +1,154 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VELOCITY_EDGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VELOCITY_EDGE_H + +#include + + +#include "local_planner/utility_tool.h" + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_vertex_timediff.h" +#include "timed_elastic_band/teb_base_eage.h" +#include "timed_elastic_band/teb_penalties.h" + +namespace roborts_local_planner { + +class VelocityEdge : public TebMultiEdgeBase<2, double> { + + public: + VelocityEdge() { + this->resize(3); + } + + void computeError() { + const TebVertexPose *conf1 = static_cast(_vertices[0]); + const TebVertexPose *conf2 = static_cast(_vertices[1]); + const TebVertexTimeDiff *deltaT = static_cast(_vertices[2]); + + const Eigen::Vector2d deltaS = conf2->estimate().GetPosition() - conf1->estimate().GetPosition(); + + double dist = deltaS.norm(); + const double angle_diff = g2o::normalize_theta(conf2->GetPose().GetTheta() - conf1->GetPose().GetTheta()); + if (config_param_->trajectory_opt().exact_arc_length() && angle_diff != 0) { + double radius = dist / (2 * sin(angle_diff / 2)); + dist = fabs(angle_diff * radius); + } + double vel = dist / deltaT->estimate(); + + vel *= LogisticSigmoid(100 * (deltaS.x() * cos(conf1->GetPose().GetTheta()) + + deltaS.y() * sin(conf1->GetPose().GetTheta()))); + + const double omega = angle_diff / deltaT->estimate(); + + _error[0] = PenaltyBoundToInterval(vel, -config_param_->kinematics_opt().max_vel_x_backwards(), + config_param_->kinematics_opt().max_vel_x(), config_param_->optimize_info().penalty_epsilon()); + _error[1] = PenaltyBoundToInterval(omega, config_param_->kinematics_opt().max_vel_theta(), + config_param_->optimize_info().penalty_epsilon()); + } + + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +class VelocityHolonomicEdge : public TebMultiEdgeBase<3, double> { + public: + + VelocityHolonomicEdge() { + this->resize(3); + } + + void computeError() { + + const TebVertexPose *conf1 = static_cast(_vertices[0]); + const TebVertexPose *conf2 = static_cast(_vertices[1]); + const TebVertexTimeDiff *deltaT = static_cast(_vertices[2]); + const Eigen::Vector2d deltaS = conf2->estimate().GetPosition() - conf1->estimate().GetPosition(); + + double cos_theta1 = std::cos(conf1->GetPose().GetTheta()); + double sin_theta1 = std::sin(conf1->GetPose().GetTheta()); + + double r_dx = cos_theta1 * deltaS.x() + sin_theta1 * deltaS.y(); + double r_dy = -sin_theta1 * deltaS.x() + cos_theta1 * deltaS.y(); + + double vx = r_dx / deltaT->estimate(); + double vy = r_dy / deltaT->estimate(); + double omega = g2o::normalize_theta(conf2->GetPose().GetTheta() - conf1->GetPose().GetTheta()) / deltaT->estimate(); + + _error[0] = PenaltyBoundToInterval(vx, -config_param_->kinematics_opt().max_vel_x_backwards(), + config_param_->kinematics_opt().max_vel_x(), config_param_->optimize_info().penalty_epsilon()); + _error[1] = PenaltyBoundToInterval(vy, config_param_->kinematics_opt().max_vel_y(), 0.0); + _error[2] = PenaltyBoundToInterval(omega, config_param_->kinematics_opt().max_vel_theta(), + config_param_->optimize_info().penalty_epsilon()); + + } + + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VELOCITY_EDGE_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_console.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_console.h new file mode 100644 index 00000000..a9b368af --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_console.h @@ -0,0 +1,252 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_CONSOLE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_CONSOLE_H + +#include +#include + +#include +#include +#include +#include + +#include "local_planner/obstacle.h" +#include "local_planner/data_base.h" +#include "local_planner/data_converter.h" +#include "local_planner/distance_calculation.h" + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_vertex_timediff.h" + + +namespace roborts_local_planner { + +typedef std::vector PoseSequence; + +typedef std::vector TimeDiffSequence; + +/** + * @brief graph vertices + */ +class TebVertexConsole { + public: + + TebVertexConsole(); + + virtual ~TebVertexConsole(); + + + PoseSequence &Poses() { return pose_vec_; }; + + const PoseSequence &Poses() const { + return pose_vec_; + }; + + TimeDiffSequence &TimeDiffs() { + return timediff_vec_; + }; + + const TimeDiffSequence &TimeDiffs() const { + return timediff_vec_; + }; + + double &TimeDiff(int index) { + return timediff_vec_.at(index)->GetDiffTime(); + } + + const double &TimeDiff(int index) const { + return timediff_vec_.at(index)->GetDiffTime(); + } + + DataBase &Pose(int index) { + return pose_vec_.at(index)->GetPose(); + } + + const DataBase &Pose(int index) const { + return pose_vec_.at(index)->GetPose(); + } + + DataBase &BackPose() { + return pose_vec_.back()->GetPose(); + } + + const DataBase &BackPose() const { + return pose_vec_.back()->GetPose(); + } + + double &BackTimeDiff() { + return timediff_vec_.back()->GetDiffTime(); + } + + const double &BackTimeDiff() const { + return timediff_vec_.back()->GetDiffTime(); + } + + TebVertexPose *PoseVertex(int index) { + return pose_vec_.at(index); + } + + TebVertexTimeDiff *TimeDiffVertex(int index) { + return timediff_vec_.at(index); + } + + void AddPose(const DataBase &pose, bool fixed = false); + + void AddPose(const Eigen::Ref &position, double theta, bool fixed = false); + + void AddTimeDiff(double dt, bool fixed = false); + + void AddPoseAndTimeDiff(const DataBase &pose, double dt); + + void AddPoseAndTimeDiff(const Eigen::Ref &position, double theta, double dt); + + void InsertPose(int index, const DataBase &pose); + + void InsertPose(int index, const Eigen::Ref &position, double theta); + + void InsertTimeDiff(int index, double dt); + + void DeletePose(int index); + + void DeletePose(int index, int number); + + void DeleteTimeDiff(int index); + + void DeleteTimeDiff(int index, int number); + + bool InitTEBtoGoal(const DataBase &start, + const DataBase &goal, + double diststep = 0, + double timestep = 1, + int min_samples = 3, + bool guess_backwards_motion = false); + + template + bool InitTEBtoGoal(BidirIter path_start, + BidirIter path_end, + Fun fun_position, + double max_vel_x, + double max_vel_theta, + boost::optional max_acc_x, + boost::optional max_acc_theta, + boost::optional start_orientation, + boost::optional goal_orientation, + int min_samples = 3, + bool guess_backwards_motion = false); + + bool InitTEBtoGoal(std::vector& plan, + double dt, + bool estimate_orient = false, + int min_samples = 3, + bool guess_backwards_motion = false, + bool micro_control = false); + + void UpdateAndPruneTEB(boost::optional new_start, + boost::optional new_goal, + int min_samples = 3); + + void AutoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples = 1000); + + void SetPoseVertexFixed(int index, bool status); + + void SetTimeDiffVertexFixed(int index, bool status); + + void ClearAllVertex(); + + int FindClosestTrajectoryPose(const Eigen::Ref &ref_point, + double *distance = NULL, + int begin_idx = 0) const; + + int FindClosestTrajectoryPose(const Eigen::Ref &ref_line_start, + const Eigen::Ref &ref_line_end, + double *distance = NULL) const; + + int FindClosestTrajectoryPose(const Point2dContainer &vertices, double *distance = NULL) const; + + int FindClosestTrajectoryPose(const Obstacle &obstacle, double *distance = NULL) const; + + int SizePoses() const { + return (int) pose_vec_.size(); + } + + int SizeTimeDiffs() const { + return (int) timediff_vec_.size(); + } + + bool IsInit() const { + return !timediff_vec_.empty() && !pose_vec_.empty(); + } + + double GetSumOfAllTimeDiffs() const; + + double GetAccumulatedDistance() const; + + bool DetectDetoursBackwards(double threshold = 0) const; + + bool IsTrajectoryInsideRegion(double radius, double max_dist_behind_robot = -1, int skip_poses = 0); + + protected: + PoseSequence pose_vec_; + TimeDiffSequence timediff_vec_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace roborts_local_planner +#include "timed_elastic_band.hpp" +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_CONSOLE_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_pose.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_pose.h new file mode 100644 index 00000000..279c1c01 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_pose.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_POSE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_POSE_H + +#include +#include +#include +#include + +#include "local_planner/data_base.h" + +namespace roborts_local_planner { +class TebVertexPose : public g2o::BaseVertex<3, DataBase> { + public: + TebVertexPose (bool fixed = false) { + setToOriginImpl(); + setFixed(fixed); + } + + TebVertexPose (const DataBase& teb_data, bool fixed =false) { + _estimate = teb_data; + setFixed(fixed); + } + + ~TebVertexPose(){} + + virtual void setToOriginImpl() { + _estimate.SetZero(); + } + + virtual void oplusImpl(const double* update) { + _estimate.Plus(update); + } + + virtual bool read(std::istream& is) { + return true; + } + + virtual bool write(std::ostream& os) const { + return true; + } + + DataBase& GetPose () { + return _estimate; + } + + const DataBase& GetPose () const { + return _estimate; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; // class TebVertexPose + +} // namespace roborts_local_planner + +#endif //ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_POSE_H \ No newline at end of file diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_timediff.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_timediff.h new file mode 100644 index 00000000..e2ea697d --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_vertex_timediff.h @@ -0,0 +1,111 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_TIMEDIFF_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_TIMEDIFF_H + + +#include "g2o/config.h" +#include "g2o/core/base_vertex.h" +#include "g2o/core/hyper_graph_action.h" +#include + +namespace roborts_local_planner { + +class TebVertexTimeDiff : public g2o::BaseVertex<1, double> { + public: + + TebVertexTimeDiff(bool fixed = false) { + setToOriginImpl(); + setFixed(fixed); + } + + TebVertexTimeDiff(double dt, bool fixed = false) { + _estimate = dt; + setFixed(fixed); + } + + ~TebVertexTimeDiff() {} + + double &GetDiffTime() { return _estimate; } + + const double &GetDiffTime() const { return _estimate; } + + virtual void setToOriginImpl() { + _estimate = 0.1; + } + + virtual void oplusImpl(const double* update) { + _estimate += *update; + } + + virtual bool read(std::istream& is) { + + } + + virtual bool write(std::ostream& os) const { + return true; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace roborts_local_planner +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VERTEX_TIMEDIFF_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_via_point_edge.h b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_via_point_edge.h new file mode 100644 index 00000000..b0e55bcf --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/teb_via_point_edge.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Notes: + * The following class is derived from a class defined by the + * g2o-framework. g2o is licensed under the terms of the BSD License. + * Refer to the base class source for detailed licensing information. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#ifndef ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VIA_POINT_EDGE_H +#define ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VIA_POINT_EDGE_H + +#include + +#include "timed_elastic_band/teb_vertex_pose.h" +#include "timed_elastic_band/teb_base_eage.h" + +namespace roborts_local_planner { + +class ViaPointEdge : public TebUnaryEdgeBase<1, const Eigen::Vector2d *, TebVertexPose> { + public: + + ViaPointEdge() { + _measurement = NULL; + } + + void computeError() { + const TebVertexPose *bandpt = static_cast(_vertices[0]); + + _error[0] = (bandpt->GetPose().GetPosition() - *_measurement).norm(); + } + + void SetViaPoint(const Eigen::Vector2d *via_point) { + _measurement = via_point; + } + + void SetParameters(const Eigen::Vector2d *via_point) { + _measurement = via_point; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +} // namespace roborts_local_planner + +#endif // ROBORTS_PLANNING_LOCAL_PLANNER_TEB_VIA_POINT_EDGE_H diff --git a/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/timed_elastic_band.hpp b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/timed_elastic_band.hpp new file mode 100644 index 00000000..89b82ba0 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/include/timed_elastic_band/timed_elastic_band.hpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include "timed_elastic_band/teb_vertex_console.h" + +namespace roborts_local_planner { + +template +bool TebVertexConsole::InitTEBtoGoal(BidirIter path_start, + BidirIter path_end, + Fun fun_position, + double max_vel_x, + double max_vel_theta, + boost::optional max_acc_x, + boost::optional max_acc_theta, + boost::optional start_orientation, + boost::optional goal_orientation, + int min_samples, + bool guess_backwards_motion) { + Eigen::Vector2d start_position = fun_position(*path_start); + Eigen::Vector2d goal_position = fun_position(*boost::prior(path_end)); + + bool backwards = false; + + double start_orient, goal_orient; + if (start_orientation) { + start_orient = *start_orientation; + + if (guess_backwards_motion + && (goal_position - start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0) + backwards = true; + } else { + Eigen::Vector2d start2goal = goal_position - start_position; + start_orient = atan2(start2goal[1], start2goal[0]); + } + double timestep = 1; + + + if (goal_orientation) { + goal_orient = *goal_orientation; + } else { + goal_orient = start_orient; + } + + if (!IsInit()) { + AddPose(start_position, start_orient, true); + + std::advance(path_start, 1); + std::advance(path_end, -1); + int idx = 0; + for (; path_start != path_end; ++path_start) + { + Eigen::Vector2d curr_point = fun_position(*path_start); + Eigen::Vector2d + diff_last = curr_point - Pose(idx).GetPosition(); + double diff_norm = diff_last.norm(); + + double timestep_vel = diff_norm / max_vel_x; + double timestep_acc; + if (max_acc_x) { + timestep_acc = sqrt(2 * diff_norm / (*max_acc_x)); + if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc; + else timestep = timestep_vel; + } else timestep = timestep_vel; + + if (timestep < 0) timestep = 0.2; + + double yaw = atan2(diff_last[1], diff_last[0]); + if (backwards) + yaw = g2o::normalize_theta(yaw + M_PI); + AddPoseAndTimeDiff(curr_point, yaw, timestep); + + Eigen::Vector2d diff_next = fun_position(*boost::next(path_start)) + - curr_point; + double ang_diff = std::abs(g2o::normalize_theta(atan2(diff_next[1], diff_next[0]) + - atan2(diff_last[1], diff_last[0]))); + + timestep_vel = ang_diff / max_vel_theta; + if (max_acc_theta) { + timestep_acc = sqrt(2 * ang_diff / (*max_acc_theta)); + if (timestep_vel < timestep_acc) timestep = timestep_acc; + else timestep = timestep_vel; + } else timestep = timestep_vel; + + if (timestep < 0) timestep = 0.2; + + yaw = atan2(diff_last[1], diff_last[0]); + if (backwards) + yaw = g2o::normalize_theta(yaw + M_PI); + AddPoseAndTimeDiff(curr_point, yaw, timestep); + + ++idx; + } + Eigen::Vector2d diff = goal_position - Pose(idx).GetPosition(); + double diff_norm = diff.norm(); + double timestep_vel = diff_norm / max_vel_x; + if (max_acc_x) { + double timestep_acc = sqrt(2 * diff_norm / (*max_acc_x)); + if (timestep_vel < timestep_acc) timestep = timestep_acc; + else timestep = timestep_vel; + } else timestep = timestep_vel; + + DataBase goal(goal_position, goal_orient); + + + if (SizePoses() < min_samples - 1) { + while (SizePoses() < min_samples - 1) { + + DataBase temp_data; + temp_data.AverageInPlace(BackPose(), goal); + + AddPoseAndTimeDiff(temp_data, timestep); + } + } + + AddPoseAndTimeDiff(goal, timestep); + SetPoseVertexFixed(SizePoses() - 1, true); + } else { + return false; + } + return true; +} + +} // namespace roborts_local_planner + diff --git a/roborts_planning/local_planner/timed_elastic_band/src/teb_local_planner.cpp b/roborts_planning/local_planner/timed_elastic_band/src/teb_local_planner.cpp new file mode 100644 index 00000000..7cee21bf --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/src/teb_local_planner.cpp @@ -0,0 +1,663 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + LOG_INFO << "Footprint model 'polygon' loaded"; + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include "timed_elastic_band/teb_local_planner.h" + + +namespace roborts_local_planner { + +TebLocalPlanner::TebLocalPlanner () { + +} + +TebLocalPlanner::~TebLocalPlanner () { + +} + +roborts_common::ErrorInfo TebLocalPlanner::ComputeVelocityCommands(roborts_msgs::TwistAccel &cmd_vel) { + + if (!is_initialized_) { + ROS_ERROR("timed_elastic_band doesn't be initialized"); + roborts_common::ErrorInfo algorithm_init_error(roborts_common::LP_ALGORITHM_INITILIZATION_ERROR, + "teb initialize failed"); + ROS_ERROR("%s",algorithm_init_error.error_msg().c_str()); + return algorithm_init_error; + } + + GetPlan(temp_plan_); + + cmd_vel.twist.linear.x = 0; + cmd_vel.twist.linear.y = 0; + cmd_vel.twist.angular.z = 0; + + cmd_vel.accel.linear.x = 0; + cmd_vel.accel.linear.y = 0; + cmd_vel.accel.angular.z = 0; + + + UpdateRobotPose(); + UpdateRobotVel(); + UpdateGlobalToPlanTranform(); + + auto time_now = std::chrono::system_clock::now(); + oscillation_time_ = std::chrono::duration_cast(time_now - oscillation_).count() / 1000.0f; + + if (oscillation_time_ > 1.0) { + if ((robot_pose_.GetPosition() - last_robot_pose_.GetPosition()).norm() < 0.1) { + local_cost_.lock()->ClearCostMap(); + } else { + oscillation_time_ = 0; + oscillation_ = std::chrono::system_clock::now(); + last_robot_pose_ = robot_pose_; + } + } + + if (IsGoalReached()) { + roborts_common::ErrorInfo algorithm_ok(roborts_common::OK, "reached the goal"); + ROS_INFO("reached the goal"); + return algorithm_ok; + } + + PruneGlobalPlan(); + + int goal_idx; + + if (!TransformGlobalPlan(&goal_idx)) { + roborts_common::ErrorInfo PlanTransformError(roborts_common::LP_PLANTRANSFORM_ERROR, "plan transform error"); + ROS_ERROR("%s", PlanTransformError.error_msg().c_str()); + return PlanTransformError; + } + + if (transformed_plan_.empty()) { + roborts_common::ErrorInfo PlanTransformError(roborts_common::LP_PLANTRANSFORM_ERROR, "transformed plan is empty"); + ROS_ERROR("transformed plan is empty"); + return PlanTransformError; + } + + /*tf::Stamped goal_point; + tf::poseStampedMsgToTF(transformed_plan_.poses.back(), goal_point); + robot_goal_.GetPosition().coeffRef(0) = goal_point.getOrigin().getX(); + robot_goal_.GetPosition().coeffRef(1) = goal_point.getOrigin().getY();*/ + + /*if (global_plan_.poses.size() - goal_idx < 5) { + robot_goal_.GetTheta() = tf::getYaw(global_plan_.poses.back().pose.orientation); + transformed_plan_.poses.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.GetTheta()); + } else*/ if (global_plan_overwrite_orientation_) { + transformed_plan_.back().SetTheta(EstimateLocalGoalOrientation(transformed_plan_.back(), goal_idx)); + } + + if (transformed_plan_.size()==1) {// plan only contains the goal + transformed_plan_.insert(transformed_plan_.begin(), robot_pose_); // insert start (not yet initialized) + } else { + transformed_plan_.front() = robot_pose_;// update start; + } + + + obst_vector_.clear(); + + + robot_goal_ = transformed_plan_.back(); +// while (obst_vector_.empty()) { +// usleep(1); + UpdateObstacleWithCostmap(robot_goal_.GetPosition()); +// } + + UpdateViaPointsContainer(); + + bool micro_control = false; + if (global_plan_.poses.back().pose.position.z == 1) { + micro_control = true; + } + + bool success = optimal_->Optimal(transformed_plan_, &robot_current_vel_, free_goal_vel_, micro_control); + + if (!success) { + optimal_->ClearPlanner(); + roborts_common::ErrorInfo OptimalError(roborts_common::LP_OPTIMAL_ERROR, "optimal error"); + ROS_ERROR("optimal error"); + last_cmd_ = cmd_vel; + return OptimalError; + } + + bool feasible = optimal_->IsTrajectoryFeasible(teb_error_info_, robot_cost_.get(), robot_footprint_, + robot_inscribed_radius_, robot_circumscribed_radius, + fesiable_step_look_ahead_); + if (!feasible) { +// cmd_vel.twist.linear.x = 0; +// cmd_vel.twist.linear.y = 0; +// cmd_vel.twist.angular.z = 0; +// +// cmd_vel.accel.linear.x = 0; +// cmd_vel.accel.linear.y = 0; +// cmd_vel.accel.angular.z = 0; + + optimal_->ClearPlanner(); + last_cmd_ = cmd_vel; + ROS_ERROR("trajectory is not feasible"); + roborts_common::ErrorInfo trajectory_error(roborts_common::LP_ALGORITHM_TRAJECTORY_ERROR, "trajectory is not feasible"); + return trajectory_error; + } + + if (!optimal_->GetVelocity(teb_error_info_, cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, + cmd_vel.accel.linear.x, cmd_vel.accel.linear.y, cmd_vel.accel.angular.z)) { + optimal_->ClearPlanner(); + ROS_ERROR("can not get the velocity"); + roborts_common::ErrorInfo velocity_error(roborts_common::LP_VELOCITY_ERROR, "velocity is not feasible"); + last_cmd_ = cmd_vel; + return velocity_error; + } + + SaturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, + max_vel_x_,max_vel_y_,max_vel_theta_,max_vel_x_backwards); + + last_cmd_ = cmd_vel; + +// cmd_vel.linear.x = 0; +// cmd_vel.linear.y = 0; +// cmd_vel.angular.z = 0; + + optimal_->Visualize(); + + ROS_INFO("compute velocity succeed"); + return roborts_common::ErrorInfo(roborts_common::ErrorCode::OK); +} + +bool TebLocalPlanner::IsGoalReached () { + + tf::Stamped global_goal; + tf::poseStampedMsgToTF(global_plan_.poses.back(), global_goal); + global_goal.setData( plan_to_global_transform_ * global_goal ); + auto goal = DataConverter::LocalConvertTFData(global_goal); + + auto distance = (goal.first - robot_pose_.GetPosition()).norm(); + double delta_orient = g2o::normalize_theta( goal.second - robot_pose_.GetTheta()); + + if (distance < xy_goal_tolerance_ + && fabs(delta_orient) < yaw_goal_tolerance_) { + ROS_INFO("goal reached"); + return true; + } else { + return false; + } + +} + +bool TebLocalPlanner::SetPlan(const nav_msgs::Path& plan, const geometry_msgs::PoseStamped& goal) { + if (plan_mutex_.try_lock()) { + ROS_INFO("set plan"); + if (plan.poses.empty()) { + temp_plan_.poses.push_back(goal); + } else { + temp_plan_ = plan; + } + plan_mutex_.unlock(); + } +} + +bool TebLocalPlanner::GetPlan(const nav_msgs::Path& plan) { + if (plan_mutex_.try_lock()) { + global_plan_ = plan; + plan_mutex_.unlock(); + } +} + +bool TebLocalPlanner::PruneGlobalPlan() { + if (global_plan_.poses.empty()) { + return true; + } + try { + tf::StampedTransform global_to_plan_transform; + tf_.lock()->lookupTransform(global_plan_.poses.front().header.frame_id, + robot_tf_pose_.frame_id_, ros::Time(0), + global_to_plan_transform); + tf::Stamped robot; + robot.setData( global_to_plan_transform * robot_tf_pose_ ); + + Eigen::Vector2d robot_to_goal(robot.getOrigin().x() - global_plan_.poses.back().pose.position.x, + robot.getOrigin().y() - global_plan_.poses.back().pose.position.y); + + + for (auto iterator = global_plan_.poses.begin(); iterator != global_plan_.poses.end(); ++iterator) { + Eigen::Vector2d temp_vector (robot.getOrigin().x() - iterator->pose.position.x, + robot.getOrigin().y() - iterator->pose.position.y); + if (temp_vector.norm() < 0.8) { + if (iterator == global_plan_.poses.begin()) { + break; + } + global_plan_.poses.erase(global_plan_.poses.begin(), iterator); + break; + } + } + } + catch (const tf::TransformException& ex) { + ROS_ERROR("prune global plan false, %s", ex.what()); + return false; + } + return true; +} + +bool TebLocalPlanner::TransformGlobalPlan(int *current_goal_idx) { + + + transformed_plan_.clear(); + + try { + if (global_plan_.poses.empty()) { + ROS_ERROR("Received plan with zero length"); + *current_goal_idx = 0; + return false; + } + UpdateGlobalToPlanTranform(); + + tf::Stamped robot_pose; + tf_.lock()->transformPose(global_plan_.poses.front().header.frame_id, robot_tf_pose_, robot_pose); + + double dist_threshold = std::max(costmap_->GetSizeXCell() * costmap_->GetResolution() / 2.0, + costmap_->GetSizeYCell() * costmap_->GetResolution() / 2.0); + dist_threshold *= 0.85; + + int i = 0; + double sq_dist_threshold = dist_threshold * dist_threshold; + double sq_dist = 1e10; + double new_sq_dist = 0; + while (i < (int)global_plan_.poses.size()) { + double x_diff = robot_pose.getOrigin().x() - global_plan_.poses[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan_.poses[i].pose.position.y; + new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (new_sq_dist > sq_dist && sq_dist < sq_dist_threshold) { + sq_dist = new_sq_dist; + break; + } + sq_dist = new_sq_dist; + ++i; + } + + tf::Stamped tf_pose; + geometry_msgs::PoseStamped newer_pose; + + double plan_length = 0; + + while(i < (int)global_plan_.poses.size() && + sq_dist <= sq_dist_threshold && (cut_lookahead_dist_<=0 || + plan_length <= cut_lookahead_dist_)) { + const geometry_msgs::PoseStamped& pose = global_plan_.poses[i]; + tf::poseStampedMsgToTF(pose, tf_pose); + tf_pose.setData(plan_to_global_transform_ * tf_pose); + tf_pose.stamp_ = plan_to_global_transform_.stamp_; + tf_pose.frame_id_ = global_frame_; + tf::poseStampedTFToMsg(tf_pose, newer_pose); + auto temp = DataConverter::LocalConvertGData(newer_pose.pose); + DataBase data_pose(temp.first, temp.second); + + transformed_plan_.push_back(data_pose); + + double x_diff = robot_pose.getOrigin().x() - global_plan_.poses[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan_.poses[i].pose.position.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + if (i>0 && cut_lookahead_dist_>0) { + plan_length += Distance(global_plan_.poses[i-1].pose.position.x, global_plan_.poses[i-1].pose.position.y, + global_plan_.poses[i].pose.position.x, global_plan_.poses[i].pose.position.y); + } + + ++i; + } + + if (transformed_plan_.empty()) { + tf::poseStampedMsgToTF(global_plan_.poses.back(), tf_pose); + tf_pose.setData(plan_to_global_transform_ * tf_pose); + tf_pose.stamp_ = plan_to_global_transform_.stamp_; + tf_pose.frame_id_ = global_frame_; + tf::poseStampedTFToMsg(tf_pose, newer_pose); + + auto temp = DataConverter::LocalConvertGData(newer_pose.pose); + DataBase data_pose(temp.first, temp.second); + + transformed_plan_.push_back(data_pose); + + if (current_goal_idx) { + *current_goal_idx = int(global_plan_.poses.size())-1; + } + } else { + if (current_goal_idx) { + *current_goal_idx = i-1; + } + } + + } + catch(tf::LookupException& ex) { + //LOG_ERROR << "transform error, " << ex.what(); + return false; + } + catch(tf::ConnectivityException& ex) { + ROS_ERROR("Connectivity Error: %s", ex.what()); + return false; + } + catch(tf::ExtrapolationException& ex) { + ROS_ERROR("Extrapolation Error: %s", ex.what()); + if (global_plan_.poses.size() > 0) { + ROS_INFO("Global Frame: %s Plan Frame size %d : %s", global_frame_.c_str(),\ + (unsigned int)global_plan_.poses.size(),\ + global_plan_.poses[0].header.frame_id.c_str()); + } + return false; + } + + return true; +} + +double TebLocalPlanner::EstimateLocalGoalOrientation(const DataBase& local_goal, + int current_goal_idx, int moving_average_length) const { + int n = (int)global_plan_.poses.size(); + + + if (current_goal_idx > n-moving_average_length-2) { + if (current_goal_idx >= n-1) { + return local_goal.GetTheta(); + } else { + tf::Quaternion global_orientation; + tf::quaternionMsgToTF(global_plan_.poses.back().pose.orientation, global_orientation); + return tf::getYaw(plan_to_global_transform_.getRotation() * global_orientation ); + } + } + + moving_average_length = std::min(moving_average_length, n-current_goal_idx-1 ); + + std::vector candidates; + tf::Stamped tf_pose_k; + tf::Stamped tf_pose_kp1; + + const geometry_msgs::PoseStamped& pose_k = global_plan_.poses.at(current_goal_idx); + tf::poseStampedMsgToTF(pose_k, tf_pose_k); + tf_pose_kp1.setData(plan_to_global_transform_ * tf_pose_k); + + int range_end = current_goal_idx + moving_average_length; + for (int i = current_goal_idx; i < range_end; ++i) { + + const geometry_msgs::PoseStamped& pose_k1 = global_plan_.poses.at(i+1); + tf::poseStampedMsgToTF(pose_k1, tf_pose_kp1); + tf_pose_kp1.setData(plan_to_global_transform_ * tf_pose_kp1); + + candidates.push_back( std::atan2(tf_pose_kp1.getOrigin().getY() - tf_pose_k.getOrigin().getY(), + tf_pose_kp1.getOrigin().getX() - tf_pose_k.getOrigin().getX() )); + + if (iGetSizeXCell()-1; ++i) { + for (unsigned int j=0; jGetSizeYCell()-1; ++j) { + if (costmap_->GetCost(i,j) == roborts_local_planner::LETHAL_OBSTACLE) { + Eigen::Vector2d obs; + costmap_->Map2World(i, j, obs.coeffRef(0), obs.coeffRef(1)); + + Eigen::Vector2d obs_dir = obs - robot_pose_.GetPosition(); + if (obs_dir.dot(goal_orient) < 0 + && obs_dir.norm() > osbtacle_behind_robot_dist_) { + continue; + } + + obst_vector_.push_back(ObstaclePtr(new PointObstacle(obs))); + } + } // for y cell size + } // for x cell size + +} + +void TebLocalPlanner::UpdateViaPointsContainer() { + + via_points_.clear(); + + double min_separation = param_config_.trajectory_opt().global_plan_viapoint_sep(); + if (min_separation<0) { + return; + } + + std::size_t prev_idx = 0; + for (std::size_t i=1; i < transformed_plan_.size(); ++i) {// skip first one, since we do not need any point before the first min_separation [m] + + if (Distance( transformed_plan_[prev_idx].GetPosition().coeff(0), transformed_plan_[prev_idx].GetPosition().coeff(1), + transformed_plan_[i].GetPosition().coeff(0), transformed_plan_[i].GetPosition().coeff(1) ) < min_separation) { + continue; + } + + auto temp = transformed_plan_[i].GetPosition(); + via_points_.push_back(temp); + prev_idx = i; + } +} + +void TebLocalPlanner::UpdateRobotPose() { + local_cost_.lock()->GetRobotPose(robot_tf_pose_); + Eigen::Vector2d position; + position.coeffRef(0) = robot_tf_pose_.getOrigin().getX(); + position.coeffRef(1) = robot_tf_pose_.getOrigin().getY(); + robot_pose_ = DataBase(position, tf::getYaw(robot_tf_pose_.getRotation())); +} + +void TebLocalPlanner::UpdateRobotVel() { + tf::Stamped robot_vel_tf; + odom_info_.GetVel(robot_vel_tf); + robot_current_vel_.linear.x = robot_vel_tf.getOrigin().getX(); + robot_current_vel_.linear.y = robot_vel_tf.getOrigin().getY(); + robot_current_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation()); +} + +void TebLocalPlanner::UpdateGlobalToPlanTranform() { + tf_.lock()->waitForTransform(global_frame_, ros::Time::now(), + global_plan_.poses.front().header.frame_id, global_plan_.poses.front().header.stamp, + global_plan_.poses.front().header.frame_id, ros::Duration(0.5)); + tf_.lock()->lookupTransform(global_frame_, ros::Time(), + global_plan_.poses.front().header.frame_id, global_plan_.poses.front().header.stamp, + global_plan_.poses.front().header.frame_id, plan_to_global_transform_); +} + +void TebLocalPlanner::SaturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, + double max_vel_theta, double max_vel_x_backwards) const { + + if (vx > max_vel_x) { + vx = max_vel_x; + } + + if (max_vel_x_backwards<=0) { + ROS_INFO("Do not choose max_vel_x_backwards to be <=0. "\ + "Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); + } else if (vx < -max_vel_x_backwards) { + vx = -max_vel_x_backwards; + } + + if (vy > max_vel_y) { + vy = max_vel_y; + } else if (vy < -max_vel_y) { + vy = -max_vel_y; + } + + if (omega > max_vel_theta) { + omega = max_vel_theta; + } else if (omega < -max_vel_theta) { + omega = -max_vel_theta; + } + +} + +double TebLocalPlanner::ConvertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const { + if (omega==0 || v==0) { + return 0; + } + + double radius = v/omega; + + if (fabs(radius) < min_turning_radius) { + radius = double(g2o::sign(radius)) * min_turning_radius; + } + + return std::atan(wheelbase / radius); +} + +roborts_common::ErrorInfo TebLocalPlanner::Initialize (std::shared_ptr local_cost, + std::shared_ptr tf, LocalVisualizationPtr visual) { + + if (!is_initialized_) { + oscillation_ = std::chrono::system_clock::now(); + tf_ = tf; + local_cost_ = local_cost; + + std::string full_path = ros::package::getPath("roborts_planning") + \ + "/local_planner/timed_elastic_band/config/timed_elastic_band.prototxt"; + roborts_common::ReadProtoFromTextFile(full_path.c_str(), ¶m_config_); + if (¶m_config_ == nullptr) { + ROS_ERROR("error occur when loading config file"); + roborts_common::ErrorInfo read_file_error(roborts_common::ErrorCode::LP_ALGORITHM_INITILIZATION_ERROR, + "load algorithm param file failed"); + is_initialized_ = false; + ROS_ERROR("%s", read_file_error.error_msg().c_str()); + return read_file_error; + } + + max_vel_x_ = param_config_.kinematics_opt().max_vel_x(); + max_vel_y_ = param_config_.kinematics_opt().max_vel_y(); + max_vel_theta_ = param_config_.kinematics_opt().max_vel_theta(); + max_vel_x_backwards = param_config_.kinematics_opt().max_vel_x_backwards(); + free_goal_vel_ = param_config_.tolerance_opt().free_goal_vel(); + + xy_goal_tolerance_ = param_config_.tolerance_opt().xy_goal_tolerance(); + yaw_goal_tolerance_ = param_config_.tolerance_opt().yaw_goal_tolerance(); + + cut_lookahead_dist_ = param_config_.trajectory_opt().max_global_plan_lookahead_dist(); + fesiable_step_look_ahead_ = param_config_.trajectory_opt().feasibility_check_no_poses(); + + osbtacle_behind_robot_dist_ = param_config_.obstacles_opt().costmap_obstacles_behind_robot_dist(); + + global_plan_overwrite_orientation_ = param_config_.trajectory_opt().global_plan_overwrite_orientation(); + + global_frame_ = local_cost_.lock()->GetGlobalFrameID(); + + visual_ = visual; + + costmap_ = local_cost_.lock()->GetLayeredCostmap()->GetCostMap(); + + robot_cost_ = std::make_shared(*costmap_); + + obst_vector_.reserve(200); + RobotFootprintModelPtr robot_model = GetRobotFootprintModel(param_config_); + optimal_ = OptimalBasePtr(new TebOptimal(param_config_, &obst_vector_, robot_model, visual_, &via_points_)); + + std::vector robot_footprint; + robot_footprint = local_cost_.lock()->GetRobotFootprint(); + //robot_footprint_ = DataConverter::LocalConvertGData(robot_footprint); + + roborts_costmap::RobotPose robot_pose; + local_cost_.lock()->GetRobotPose(robot_pose); + auto temp_pose = DataConverter::LocalConvertRMData(robot_pose); + robot_pose_ = DataBase(temp_pose.first, temp_pose.second); + robot_footprint_.push_back(robot_pose_.GetPosition()); + last_robot_pose_ = robot_pose_; + + RobotPositionCost::CalculateMinAndMaxDistances(robot_footprint_, + robot_inscribed_radius_, + robot_circumscribed_radius); + odom_info_.SetTopic(param_config_.opt_frame().odom_frame()); + + is_initialized_ = true; + ROS_INFO("local algorithm initialize ok"); + return roborts_common::ErrorInfo(roborts_common::ErrorCode::OK); + } + + return roborts_common::ErrorInfo(roborts_common::ErrorCode::OK); +} + +void TebLocalPlanner::RegisterErrorCallBack(ErrorInfoCallback error_callback) { + error_callback_ = error_callback; +} + +bool TebLocalPlanner::CutAndTransformGlobalPlan(int *current_goal_idx) { + if (!transformed_plan_.empty()) { + transformed_plan_.clear(); + } + +} + +bool TebLocalPlanner::SetPlanOrientation() { + if (global_plan_.poses.size() < 2) { + ROS_WARN("can not compute the orientation because the global plan size is: %d", (int)global_plan_.poses.size()); + return false; + } else { + //auto goal = DataConverter::LocalConvertGData(global_plan_.poses.back().pose); + //auto line_vector = (robot_pose_.GetPosition() - goal.first); + //auto orientation = GetOrientation(line_vector); + for (int i = 0; i < global_plan_.poses.size() - 1; ++i) { + auto pose = DataConverter::LocalConvertGData(global_plan_.poses[i].pose); + auto next_pose = DataConverter::LocalConvertGData(global_plan_.poses[i+1].pose); + double x = global_plan_.poses[i+1].pose.position.x - global_plan_.poses[i].pose.position.x; + double y = global_plan_.poses[i+1].pose.position.y - global_plan_.poses[i].pose.position.y; + double angle = atan2(y, x); + auto quaternion = EulerToQuaternion(0, 0, angle); + global_plan_.poses[i].pose.orientation.w = quaternion[0]; + global_plan_.poses[i].pose.orientation.x = quaternion[1]; + global_plan_.poses[i].pose.orientation.y = quaternion[2]; + global_plan_.poses[i].pose.orientation.z = quaternion[3]; + } + } +} + +} // namespace roborts_local_planner diff --git a/roborts_planning/local_planner/timed_elastic_band/src/teb_optimal.cpp b/roborts_planning/local_planner/timed_elastic_band/src/teb_optimal.cpp new file mode 100644 index 00000000..2ad24d72 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/src/teb_optimal.cpp @@ -0,0 +1,1039 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include "timed_elastic_band/teb_optimal.h" + +namespace roborts_local_planner { +TebOptimal::TebOptimal() : obstacles_(NULL), visualization_(NULL),via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::NONE) + ,robot_model_(new PointRobotFootprint()), initialized_(false), optimized_(false) { + +} + +TebOptimal::TebOptimal(const Config& config_param, + ObstContainer *obstacles, + RobotFootprintModelPtr robot_model, + LocalVisualizationPtr visual, + const ViaPointContainer *via_points) { + initialize(config_param, obstacles, robot_model, visual,via_points); +} + +void TebOptimal::initialize(const Config& config_param, + ObstContainer *obstacles, + RobotFootprintModelPtr robot_model, + LocalVisualizationPtr visual, + const ViaPointContainer *via_points) { + + optimizer_ = InitOptimizer(); + + obstacles_ = obstacles; + robot_model_ = robot_model; + visualization_ = visual; + via_points_ = via_points; + cost_ = HUGE_VAL; + + vel_start_.first = true; + vel_start_.second.linear.x = 0; + vel_start_.second.linear.y = 0; + vel_start_.second.angular.z = 0; + + vel_end_.first = true; + vel_end_.second.linear.x = 0; + vel_end_.second.linear.y = 0; + vel_end_.second.angular.z = 0; + + param_config_ = config_param; + robot_info_ = config_param.kinematics_opt(); + obstacles_info_ = config_param.obstacles_opt(); + trajectory_info_ = config_param.trajectory_opt(); + optimization_info_ = config_param.optimize_info(); + + + initialized_ = true; +} + +void TebOptimal::RegisterG2OTypes() { + g2o::Factory* factory = g2o::Factory::instance(); + factory->registerType("TEB_VERTEX_POSE", new g2o::HyperGraphElementCreator); + factory->registerType("TEB_VERTEX_TIMEDIFF", new g2o::HyperGraphElementCreator); + + factory->registerType("TIME_OPTIMAL_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("VELOCITY_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("VELOCITY_HOLONOMIC_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("ACCELERATION_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("ACCELERATION_START_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("ACCELERATION_GOAL_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("ACCELERATION_HOLONOMIC_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("ACCELERATION_HOLONOMIC_START_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("ACCELERATION_HOLONOMIC_GOAL_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("KINEMATICS_DIFF_DRIVE_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("KINEMATICS_CARLIKE_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("OBSTACLE_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("INFLATED_OBSTACLE_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("VIA_POINT_EDGE", new g2o::HyperGraphElementCreator); + factory->registerType("PREFER_ROTDIR_EDGE", new g2o::HyperGraphElementCreator); + return; +} + +boost::shared_ptr TebOptimal::InitOptimizer() { + + static boost::once_flag flag = BOOST_ONCE_INIT; + boost::call_once(&RegisterG2OTypes, flag); + + + boost::shared_ptr optimizer = boost::make_shared(); + TebLinearSolver* linearSolver = new TebLinearSolver(); + linearSolver->setBlockOrdering(true); + TebBlockSolver* blockSolver = new TebBlockSolver(linearSolver); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver); + + optimizer->setAlgorithm(solver); + + optimizer->initMultiThreading(); + + return optimizer; +} + +bool TebOptimal::OptimizeTeb(int iterations_innerloop, + int iterations_outerloop, + bool compute_cost_afterwards, + double obst_cost_scale, + double viapoint_cost_scale, + bool alternative_time_cost) { + if (optimization_info_.optimization_activate() == false){ + return false; + } + + bool success = false; + optimized_ = false; + + double weight_multiplier = 1.0; + + for (int i=0; i 0) { + visualization_->PublishLocalPlan(vertex_console_); + } + +} + +void TebOptimal::SetVelocityStart(const geometry_msgs::Twist &vel_start) { + vel_start_.first = true; + vel_start_.second.linear.x = vel_start.linear.x; + vel_start_.second.linear.y = vel_start.linear.y; + vel_start_.second.angular.z = vel_start.angular.z; +} + +void TebOptimal::SetVelocityEnd(const geometry_msgs::Twist &vel_end) { + vel_end_.first = true; + vel_end_.second = vel_end; +} + +bool TebOptimal::Optimal(std::vector &initial_plan, + const geometry_msgs::Twist *start_vel, + bool free_goal_vel, bool micro_control) { + if (!initialized_) { + ROS_ERROR("optimal not be initialized"); + } + + if (!vertex_console_.IsInit()) { + vertex_console_.InitTEBtoGoal(initial_plan, trajectory_info_.dt_ref(), + trajectory_info_.global_plan_overwrite_orientation(), + trajectory_info_.min_samples(), + trajectory_info_.allow_init_with_backwards_motion(), + micro_control); + + } else { + DataBase start = initial_plan.front(); + DataBase goal = initial_plan.back(); + + if ( (vertex_console_.SizePoses() > 0) + && (goal.GetPosition() - vertex_console_.BackPose().GetPosition()).norm() + < trajectory_info_.force_reinit_new_goal_dist()) { + vertex_console_.UpdateAndPruneTEB(start, goal, trajectory_info_.min_samples()); + + } else { + vertex_console_.ClearAllVertex(); + vertex_console_.InitTEBtoGoal(initial_plan, trajectory_info_.dt_ref(), + trajectory_info_.global_plan_overwrite_orientation(), + trajectory_info_.min_samples(), + trajectory_info_.allow_init_with_backwards_motion(), + micro_control); + } + } + if (start_vel) { + SetVelocityStart(*start_vel); + } + + if (free_goal_vel) { + SetVelocityGoalFree(); + } else { + vel_end_.first = true; + } + + return OptimizeTeb(optimization_info_.no_inner_iterations(), + optimization_info_.no_outer_iterations()); +} + +bool TebOptimal::Optimal(const DataBase &start, + const DataBase &goal, + const geometry_msgs::Twist *start_vel, + bool free_goal_vel, bool micro_control) { + if (!initialized_) { + ROS_ERROR("optimal not be initialized"); + } + if (!vertex_console_.IsInit()) { + vertex_console_.InitTEBtoGoal(start, goal, 0, 1, trajectory_info_.min_samples(), + trajectory_info_.allow_init_with_backwards_motion()); + } else { + if (vertex_console_.SizePoses()>0 && + (goal.GetPosition() - vertex_console_.BackPose().GetPosition()).norm() + < trajectory_info_.force_reinit_new_goal_dist()) { + vertex_console_.UpdateAndPruneTEB(start, goal, trajectory_info_.min_samples()); + } else { + vertex_console_.ClearAllVertex(); + vertex_console_.InitTEBtoGoal(start, goal, 0, 1, trajectory_info_.min_samples(), + trajectory_info_.allow_init_with_backwards_motion()); + } + } + if (start_vel){ + SetVelocityStart(*start_vel); + } + + if (free_goal_vel){ + SetVelocityGoalFree(); + } else { + vel_end_.first = true; + } + + return OptimizeTeb(optimization_info_.no_inner_iterations(), + optimization_info_.no_outer_iterations()); +} + +bool TebOptimal::BuildGraph(double weight_multiplier) { + if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) { + ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!"); + return false; + } + + AddTebVertices(); + if (obstacles_info_.legacy_obstacle_association()) { + AddObstacleLegacyEdges(weight_multiplier); + } else { + AddObstacleEdges(weight_multiplier); + } + + //AddEdgesDynamicObstacles(); + + //AddViaPointsEdges(); + + AddVelocityEdges(); + + AddAccelerationEdges(); + + AddTimeOptimalEdges(); + + if (robot_info_.min_turning_radius() == 0 || + optimization_info_.weight_kinematics_turning_radius() == 0) { + AddKinematicsDiffDriveEdges(); + } else { + AddKinematicsCarlikeEdges(); + } + + AddPreferRotDirEdges(); + + return true; +} + +bool TebOptimal::OptimizeGraph(int no_iterations, bool clear_after) { + if (robot_info_.max_vel_x() < 0.01) { + ROS_WARN("Robot Max Velocity is smaller than 0.01m/s"); + if (clear_after) { + ClearGraph(); + } + return false; + } + + if (!vertex_console_.IsInit() || vertex_console_.SizePoses() < trajectory_info_.min_samples()) { + ROS_WARN("teb size is too small"); + if (clear_after) { + ClearGraph(); + } + return false; + } + + optimizer_->setVerbose(optimization_info_.optimization_verbose()); + optimizer_->initializeOptimization(); + + int iter = optimizer_->optimize(no_iterations); + if(!iter) { + ROS_ERROR("optimize failed"); + return false; + } + if (clear_after) { + ClearGraph(); + } + + return true; +} + +void TebOptimal::ClearGraph() { + optimizer_->vertices().clear(); + optimizer_->clear(); +} + +void TebOptimal::AddTebVertices() { + unsigned int id_counter = 0; + for (int i=0; isetId(id_counter++); + optimizer_->addVertex(vertex_console_.PoseVertex(i)); + if (vertex_console_.SizeTimeDiffs()!=0 && isetId(id_counter++); + optimizer_->addVertex(vertex_console_.TimeDiffVertex(i)); + } + } +} + +void TebOptimal::AddObstacleEdges(double weight_multiplier) { + if (optimization_info_.weight_obstacle() == 0 || weight_multiplier==0 || obstacles_==nullptr ){ + return; + } + + bool inflated = obstacles_info_.inflation_dist() > obstacles_info_.min_obstacle_dist(); + + Eigen::Matrix information; + information.fill(optimization_info_.weight_obstacle() * weight_multiplier); + + Eigen::Matrix information_inflated; + information_inflated(0,0) = optimization_info_.weight_obstacle() * weight_multiplier; + information_inflated(1,1) = optimization_info_.weight_inflation(); + information_inflated(0,1) = information_inflated(1,0) = 0; + + for (int i=1; i < vertex_console_.SizePoses()-1; ++i) { + double left_min_dist = std::numeric_limits::max(); + double right_min_dist = std::numeric_limits::max(); + Obstacle* left_obstacle = nullptr; + Obstacle* right_obstacle = nullptr; + + std::vector relevant_obstacles; + + const Eigen::Vector2d pose_orient = vertex_console_.Pose(i).OrientationUnitVec(); + + + for (const ObstaclePtr& obst : *obstacles_) { + double dist = obst->GetMinimumDistance(vertex_console_.Pose(i).GetPosition()); + if (dist < obstacles_info_.min_obstacle_dist() * + obstacles_info_.obstacle_association_force_inclusion_factor()) { + relevant_obstacles.push_back(obst.get()); + continue; + } + if (dist > obstacles_info_.min_obstacle_dist() * + obstacles_info_.obstacle_association_cutoff_factor()) { + continue; + } + + if (Cross2D(pose_orient, obst->GetCentroid()) > 0) { + if (dist < left_min_dist) { + left_min_dist = dist; + left_obstacle = obst.get(); + } + } else { + if (dist < right_min_dist) { + right_min_dist = dist; + right_obstacle = obst.get(); + } + } + } + + if (left_obstacle) { + if (inflated) { + InflatedObstacleEdge* dist_bandpt_obst = new InflatedObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->SetParameters(param_config_,robot_model_.get(), left_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } else { + ObstacleEdge* dist_bandpt_obst = new ObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->SetParameters(param_config_,robot_model_.get(), left_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + } + + if (right_obstacle) { + if (inflated) { + InflatedObstacleEdge* dist_bandpt_obst = new InflatedObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->SetParameters(param_config_, robot_model_.get(), right_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } else { + ObstacleEdge* dist_bandpt_obst = new ObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->SetParameters(param_config_, robot_model_.get(), right_obstacle); + optimizer_->addEdge(dist_bandpt_obst); + } + } + + for (const Obstacle* obst : relevant_obstacles) { + if (inflated) { + InflatedObstacleEdge* dist_bandpt_obst = new InflatedObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->SetParameters(param_config_, robot_model_.get(), obst); + optimizer_->addEdge(dist_bandpt_obst); + } else { + ObstacleEdge* dist_bandpt_obst = new ObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(i)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->SetParameters(param_config_, robot_model_.get(), obst); + optimizer_->addEdge(dist_bandpt_obst); + } + } + } +} + +void TebOptimal::AddObstacleLegacyEdges(double weight_multiplier) { + if (optimization_info_.weight_obstacle()==0 || weight_multiplier==0 || obstacles_==nullptr) { + return; + } + + Eigen::Matrix information; + information.fill(optimization_info_.weight_obstacle() * weight_multiplier); + + Eigen::Matrix information_inflated; + information_inflated(0,0) = optimization_info_.weight_obstacle() * weight_multiplier; + information_inflated(1,1) = optimization_info_.weight_inflation(); + information_inflated(0,1) = information_inflated(1,0) = 0; + + bool inflated = obstacles_info_.inflation_dist() > obstacles_info_.min_obstacle_dist(); + + for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) { + if ((*obst)->IsDynamic()){ + continue; + } + int index; + if (obstacles_info_.obstacle_poses_affected() >= vertex_console_.SizePoses()){ + index = vertex_console_.SizePoses() / 2; + } else{ + index = vertex_console_.FindClosestTrajectoryPose(*(obst->get())); + } + + if ( (index <= 1) || (index > vertex_console_.SizePoses()-2) ){ + continue; + } + + if (inflated) { + InflatedObstacleEdge* dist_bandpt_obst = new InflatedObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(index)); + dist_bandpt_obst->setInformation(information_inflated); + dist_bandpt_obst->SetParameters(param_config_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst); + } else { + ObstacleEdge* dist_bandpt_obst = new ObstacleEdge; + dist_bandpt_obst->setVertex(0,vertex_console_.PoseVertex(index)); + dist_bandpt_obst->setInformation(information); + dist_bandpt_obst->SetParameters(param_config_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst); + } + + for (int neighbourIdx=0; neighbourIdx < floor(obstacles_info_.obstacle_poses_affected()/2); neighbourIdx++) { + if (index+neighbourIdx < vertex_console_.SizePoses()) { + if (inflated) { + InflatedObstacleEdge* dist_bandpt_obst_n_r = new InflatedObstacleEdge; + dist_bandpt_obst_n_r->setVertex(0,vertex_console_.PoseVertex(index+neighbourIdx)); + dist_bandpt_obst_n_r->setInformation(information_inflated); + dist_bandpt_obst_n_r->SetParameters(param_config_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_r); + } else { + ObstacleEdge* dist_bandpt_obst_n_r = new ObstacleEdge; + dist_bandpt_obst_n_r->setVertex(0,vertex_console_.PoseVertex(index+neighbourIdx)); + dist_bandpt_obst_n_r->setInformation(information); + dist_bandpt_obst_n_r->SetParameters(param_config_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_r); + } + } + if ( index - neighbourIdx >= 0) { + if (inflated) { + InflatedObstacleEdge* dist_bandpt_obst_n_l = new InflatedObstacleEdge; + dist_bandpt_obst_n_l->setVertex(0,vertex_console_.PoseVertex(index-neighbourIdx)); + dist_bandpt_obst_n_l->setInformation(information_inflated); + dist_bandpt_obst_n_l->SetParameters(param_config_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_l); + } else { + ObstacleEdge* dist_bandpt_obst_n_l = new ObstacleEdge; + dist_bandpt_obst_n_l->setVertex(0,vertex_console_.PoseVertex(index-neighbourIdx)); + dist_bandpt_obst_n_l->setInformation(information); + dist_bandpt_obst_n_l->SetParameters(param_config_, robot_model_.get(), obst->get()); + optimizer_->addEdge(dist_bandpt_obst_n_l); + } + } + } + } +} + +void TebOptimal::AddViaPointsEdges() { + if (optimization_info_.weight_viapoint()==0 || via_points_==NULL || via_points_->empty() ) { + return; + } + int start_pose_idx = 0; + int n = vertex_console_.SizePoses(); + if (n<3) { + return; + } + + for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it) { + + int index = vertex_console_.FindClosestTrajectoryPose(*vp_it, NULL, start_pose_idx); + if (trajectory_info_.via_points_ordered()){ + start_pose_idx = index+2; + } + + if ( index > n-2 ){ + index = n-2; + } + + if ( index < 1){ + index = 1; + } + + Eigen::Matrix information; + information.fill(optimization_info_.weight_viapoint()); + + ViaPointEdge* edge_viapoint = new ViaPointEdge; + edge_viapoint->setVertex(0,vertex_console_.PoseVertex(index)); + edge_viapoint->setInformation(information); + edge_viapoint->SetParameters(&(*vp_it)); + optimizer_->addEdge(edge_viapoint); + } +} + +void TebOptimal::AddVelocityEdges() { + if (robot_info_.max_vel_y() == 0) { + if ( optimization_info_.weight_max_vel_x()==0 && optimization_info_.weight_max_vel_theta()==0){ + return; + } + + int n = vertex_console_.SizePoses(); + Eigen::Matrix information; + information(0,0) = optimization_info_.weight_max_vel_x(); + information(1,1) = optimization_info_.weight_max_vel_theta(); + information(0,1) = 0.0; + information(1,0) = 0.0; + + for (int i=0; i < n - 1; ++i) { + VelocityEdge* velocity_edge = new VelocityEdge; + velocity_edge->setVertex(0,vertex_console_.PoseVertex(i)); + velocity_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + velocity_edge->setVertex(2,vertex_console_.TimeDiffVertex(i)); + velocity_edge->setInformation(information); + velocity_edge->SetConfig(param_config_); + optimizer_->addEdge(velocity_edge); + } + } else { + if ( optimization_info_.weight_max_vel_x()==0 && optimization_info_.weight_max_vel_y()==0 + && optimization_info_.weight_max_vel_theta()==0) { + return; + } + + int n = vertex_console_.SizePoses(); + Eigen::Matrix information; + information.fill(0); + information(0,0) = optimization_info_.weight_max_vel_x(); + information(1,1) = optimization_info_.weight_max_vel_y(); + information(2,2) = optimization_info_.weight_max_vel_theta(); + + for (int i=0; i < n - 1; ++i) { + VelocityHolonomicEdge* velocity_edge = new VelocityHolonomicEdge; + velocity_edge->setVertex(0,vertex_console_.PoseVertex(i)); + velocity_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + velocity_edge->setVertex(2,vertex_console_.TimeDiffVertex(i)); + velocity_edge->setInformation(information); + velocity_edge->SetConfig(param_config_); + optimizer_->addEdge(velocity_edge); + } + } +} + +void TebOptimal::AddAccelerationEdges() { + if (optimization_info_.weight_acc_lim_x()==0 && optimization_info_.weight_acc_lim_theta()==0){ + return; + } + + int n = vertex_console_.SizePoses(); + + if (robot_info_.max_vel_y() == 0 || robot_info_.acc_lim_y() == 0) { + Eigen::Matrix information; + information.fill(0); + information(0,0) = optimization_info_.weight_acc_lim_x(); + information(1,1) = optimization_info_.weight_acc_lim_theta(); + + if (vel_start_.first) { + AccelerationStartEdge* acceleration_edge = new AccelerationStartEdge; + acceleration_edge->setVertex(0,vertex_console_.PoseVertex(0)); + acceleration_edge->setVertex(1,vertex_console_.PoseVertex(1)); + acceleration_edge->setVertex(2,vertex_console_.TimeDiffVertex(0)); + acceleration_edge->SetInitialVelocity(vel_start_.second); + acceleration_edge->setInformation(information); + acceleration_edge->SetConfig(param_config_); + optimizer_->addEdge(acceleration_edge); + } + + for (int i=0; i < n - 2; ++i) { + AccelerationEdge* acceleration_edge = new AccelerationEdge; + acceleration_edge->setVertex(0,vertex_console_.PoseVertex(i)); + acceleration_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + acceleration_edge->setVertex(2,vertex_console_.PoseVertex(i+2)); + acceleration_edge->setVertex(3,vertex_console_.TimeDiffVertex(i)); + acceleration_edge->setVertex(4,vertex_console_.TimeDiffVertex(i+1)); + acceleration_edge->setInformation(information); + acceleration_edge->SetConfig(param_config_); + optimizer_->addEdge(acceleration_edge); + } + + if (vel_end_.first) { + AccelerationGoalEdge* acceleration_edge = new AccelerationGoalEdge; + acceleration_edge->setVertex(0,vertex_console_.PoseVertex(n-2)); + acceleration_edge->setVertex(1,vertex_console_.PoseVertex(n-1)); + acceleration_edge->setVertex(2,vertex_console_.TimeDiffVertex( vertex_console_.SizeTimeDiffs()-1 )); + acceleration_edge->SetGoalVelocity(vel_end_.second); + acceleration_edge->setInformation(information); + acceleration_edge->SetConfig(param_config_); + optimizer_->addEdge(acceleration_edge); + } + } else { + Eigen::Matrix information; + information.fill(0); + information(0,0) = optimization_info_.weight_acc_lim_x(); + information(1,1) = optimization_info_.weight_acc_lim_y(); + information(2,2) = optimization_info_.weight_acc_lim_theta(); + + if (vel_start_.first) { + AccelerationHolonomicStartEdge* acceleration_edge = new AccelerationHolonomicStartEdge; + acceleration_edge->setVertex(0,vertex_console_.PoseVertex(0)); + acceleration_edge->setVertex(1,vertex_console_.PoseVertex(1)); + acceleration_edge->setVertex(2,vertex_console_.TimeDiffVertex(0)); + acceleration_edge->setInitialVelocity(vel_start_.second); + acceleration_edge->setInformation(information); + acceleration_edge->SetConfig(param_config_); + optimizer_->addEdge(acceleration_edge); + } + + for (int i=0; i < n - 2; ++i) { + AccelerationHolonomicEdge* acceleration_edge = new AccelerationHolonomicEdge; + acceleration_edge->setVertex(0,vertex_console_.PoseVertex(i)); + acceleration_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + acceleration_edge->setVertex(2,vertex_console_.PoseVertex(i+2)); + acceleration_edge->setVertex(3,vertex_console_.TimeDiffVertex(i)); + acceleration_edge->setVertex(4,vertex_console_.TimeDiffVertex(i+1)); + acceleration_edge->setInformation(information); + acceleration_edge->SetConfig(param_config_); + optimizer_->addEdge(acceleration_edge); + } + + if (vel_end_.first) { + AccelerationHolonomicGoalEdge* acceleration_edge = new AccelerationHolonomicGoalEdge; + acceleration_edge->setVertex(0,vertex_console_.PoseVertex(n-2)); + acceleration_edge->setVertex(1,vertex_console_.PoseVertex(n-1)); + acceleration_edge->setVertex(2,vertex_console_.TimeDiffVertex( vertex_console_.SizeTimeDiffs()-1 )); + acceleration_edge->SetGoalVelocity(vel_end_.second); + acceleration_edge->setInformation(information); + acceleration_edge->SetConfig(param_config_); + optimizer_->addEdge(acceleration_edge); + } + } +} + +void TebOptimal::AddTimeOptimalEdges() { + if (optimization_info_.weight_optimaltime()==0){ + return; + } + + Eigen::Matrix information; + information.fill(optimization_info_.weight_optimaltime()); + + for (int i=0; i < vertex_console_.SizeTimeDiffs(); ++i) { + TimeOptimalEdge* timeoptimal_edge = new TimeOptimalEdge; + timeoptimal_edge->setVertex(0,vertex_console_.TimeDiffVertex(i)); + timeoptimal_edge->setInformation(information); + timeoptimal_edge->SetConfig(param_config_); + optimizer_->addEdge(timeoptimal_edge); + } +} + +void TebOptimal::AddKinematicsDiffDriveEdges() { + if (optimization_info_.weight_kinematics_nh()==0 && optimization_info_.weight_kinematics_forward_drive()==0) { + return; + } + + Eigen::Matrix information_kinematics; + information_kinematics.fill(0.0); + information_kinematics(0, 0) = optimization_info_.weight_kinematics_nh(); + information_kinematics(1, 1) = optimization_info_.weight_kinematics_forward_drive(); + + for (int i=0; i < vertex_console_.SizePoses()-1; i++) { + KinematicsDiffDriveEdge* kinematics_edge = new KinematicsDiffDriveEdge; + kinematics_edge->setVertex(0,vertex_console_.PoseVertex(i)); + kinematics_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + kinematics_edge->setInformation(information_kinematics); + kinematics_edge->SetConfig(param_config_); + optimizer_->addEdge(kinematics_edge); + } +} + +void TebOptimal::AddKinematicsCarlikeEdges() { + if (optimization_info_.weight_kinematics_nh()==0 && optimization_info_.weight_kinematics_turning_radius()){ + return; + } + + Eigen::Matrix information_kinematics; + information_kinematics.fill(0.0); + information_kinematics(0, 0) = optimization_info_.weight_kinematics_nh(); + information_kinematics(1, 1) = optimization_info_.weight_kinematics_turning_radius(); + + for (int i=0; i < vertex_console_.SizePoses()-1; i++) { + KinematicsCarlikeEdge* kinematics_edge = new KinematicsCarlikeEdge; + kinematics_edge->setVertex(0,vertex_console_.PoseVertex(i)); + kinematics_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + kinematics_edge->setInformation(information_kinematics); + kinematics_edge->SetConfig(param_config_); + optimizer_->addEdge(kinematics_edge); + } +} + +void TebOptimal::AddPreferRotDirEdges() { + if (prefer_rotdir_ == RotType::NONE || optimization_info_.weight_prefer_rotdir()==0) { + return; + } + + Eigen::Matrix information_rotdir; + information_rotdir.fill(optimization_info_.weight_prefer_rotdir()); + + for (int i=0; i < vertex_console_.SizePoses()-1 && i < 3; ++i) { + PreferRotDirEdge* rotdir_edge = new PreferRotDirEdge; + rotdir_edge->setVertex(0,vertex_console_.PoseVertex(i)); + rotdir_edge->setVertex(1,vertex_console_.PoseVertex(i+1)); + rotdir_edge->setInformation(information_rotdir); + + if (prefer_rotdir_ == RotType::LEFT) { + rotdir_edge->PreferLeft(); + } else { + rotdir_edge->PreferRight(); + } + optimizer_->addEdge(rotdir_edge); + } +} + +void TebOptimal::ComputeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) { + bool graph_exist_flag(false); + if (optimizer_->edges().empty() && optimizer_->vertices().empty()) { + BuildGraph(); + optimizer_->initializeOptimization(); + } else { + graph_exist_flag = true; + } + + optimizer_->computeInitialGuess(); + + cost_ = 0; + + if (alternative_time_cost) { + cost_ += vertex_console_.GetSumOfAllTimeDiffs(); + } + + for (std::vector::const_iterator it = optimizer_->activeEdges().begin(); + it!= optimizer_->activeEdges().end(); it++) { + TimeOptimalEdge* edge_time_optimal = dynamic_cast(*it); + if (edge_time_optimal!=NULL && !alternative_time_cost) { + cost_ += edge_time_optimal->GetError().squaredNorm(); + continue; + } + + KinematicsDiffDriveEdge* edge_kinematics_dd = dynamic_cast(*it); + if (edge_kinematics_dd!=NULL) { + cost_ += edge_kinematics_dd->GetError().squaredNorm(); + continue; + } + + KinematicsCarlikeEdge* edge_kinematics_cl = dynamic_cast(*it); + if (edge_kinematics_cl!=NULL) { + cost_ += edge_kinematics_cl->GetError().squaredNorm(); + continue; + } + + VelocityEdge* edge_velocity = dynamic_cast(*it); + if (edge_velocity!=NULL) { + cost_ += edge_velocity->GetError().squaredNorm(); + continue; + } + + AccelerationEdge* edge_acceleration = dynamic_cast(*it); + if (edge_acceleration!=NULL) { + cost_ += edge_acceleration->GetError().squaredNorm(); + continue; + } + + ObstacleEdge* edge_obstacle = dynamic_cast(*it); + if (edge_obstacle!=NULL) { + cost_ += edge_obstacle->GetError().squaredNorm() * obst_cost_scale; + continue; + } + InflatedObstacleEdge* edge_inflated_obstacle = dynamic_cast(*it); + + if (edge_inflated_obstacle!=NULL) { + cost_ += std::sqrt(std::pow(edge_inflated_obstacle->GetError()[0],2) * obst_cost_scale + + std::pow(edge_inflated_obstacle->GetError()[1],2)); + continue; + } + + ViaPointEdge* edge_viapoint = dynamic_cast(*it); + if (edge_viapoint!=NULL) { + cost_ += edge_viapoint->GetError().squaredNorm() * viapoint_cost_scale; + continue; + } + } + + if (!graph_exist_flag){ + ClearGraph(); + } +} + +bool TebOptimal::IsTrajectoryFeasible(roborts_common::ErrorInfo &error_info, RobotPositionCost* position_cost, const std::vector& footprint_spec, + double inscribed_radius, double circumscribed_radius, int look_ahead_idx) { + + if (look_ahead_idx < 0 || look_ahead_idx >= vertex_console_.SizePoses()) { + look_ahead_idx = vertex_console_.SizePoses() - 1; + } + + for (int i=0; i <= look_ahead_idx; ++i) { + auto position = vertex_console_.Pose(i).GetPosition(); + if ( position_cost->FootprintCost(position.coeffRef(0), position.coeffRef(1), + vertex_console_.Pose(i).GetTheta(), footprint_spec, + inscribed_radius, circumscribed_radius) < 0 ){ + return false; + } + + if (i inscribed_radius) { + DataBase center; + center.AverageInPlace(vertex_console_.Pose(i), vertex_console_.Pose(i + 1)); + if ( position_cost->FootprintCost(center.GetPosition().coeffRef(0), center.GetPosition().coeffRef(1), + center.GetTheta(), footprint_spec, + inscribed_radius, circumscribed_radius) < 0 ){ + return false; + } + } + + } + } + return true; +} + +bool TebOptimal::IsHorizonReductionAppropriate(const std::vector &initial_plan) const { + if (vertex_console_.SizePoses() < int( 1.5*double(trajectory_info_.min_samples()) ) ){ + return false; + } + + double dist = 0; + for (int i=1; i < vertex_console_.SizePoses(); ++i) { + dist += (vertex_console_.Pose(i).GetPosition() - vertex_console_.Pose(i-1).GetPosition()).norm(); + if (dist > 2){ + break; + } + } + if (dist <= 2){ + return false; + } + + if ( std::abs(g2o::normalize_theta( vertex_console_.Pose(0).GetTheta() - vertex_console_.BackPose().GetTheta() ) ) > M_PI/2) { + return true; + } + + if (vertex_console_.Pose(0).OrientationUnitVec().dot(vertex_console_.BackPose().GetPosition() - vertex_console_.Pose(0).GetPosition()) < 0) { + return true; + } + + int idx=0; + for (; idx < (int)initial_plan.size(); ++idx) { + if ( std::sqrt(std::pow(initial_plan[idx].GetPosition().coeff(0)-vertex_console_.Pose(0).GetPosition().x(), 2) + + std::pow(initial_plan[idx].GetPosition().coeff(1)-vertex_console_.Pose(0).GetPosition().y(), 2)) ) { + break; + } + } + double ref_path_length = 0; + for (; idx < int(initial_plan.size())-1; ++idx) { + ref_path_length += std::sqrt(std::pow(initial_plan[idx+1].GetPosition().coeff(0)-initial_plan[idx].GetPosition().coeff(0), 2) + + std::pow(initial_plan[idx+1].GetPosition().coeff(1)-initial_plan[idx].GetPosition().coeff(1), 2) ); + } + + double teb_length = 0; + for (int i = 1; i < vertex_console_.SizePoses(); ++i ) { + double dist = (vertex_console_.Pose(i).GetPosition() - vertex_console_.Pose(i-1).GetPosition()).norm(); + if (dist > 0.95 * obstacles_info_.min_obstacle_dist()) { + return true; + } + ref_path_length += dist; + } + if (ref_path_length>0 && teb_length/ref_path_length < 0.7) { + return true; + } + + return false; +} + +bool TebOptimal::GetVelocity(roborts_common::ErrorInfo &error_info, double &vx, double &vy, double &omega, + double &acc_x, double &acc_y, double &acc_omega) const { + + if (vertex_console_.SizePoses()<2) { + ROS_ERROR("pose is too less to compute the velocity"); + vx = 0; + vy = 0; + omega = 0; + return false; + } + + double dt = vertex_console_.TimeDiff(0); + if (dt<=0) { + ROS_ERROR("the time between two pose is nagetive"); + vx = 0; + vy = 0; + omega = 0; + return false; + } + + ExtractVelocity(vertex_console_.Pose(0), vertex_console_.Pose(1), dt, vx, vy, omega); + + double dt_2 = vertex_console_.TimeDiff(1); + double vx_2, vy_2, omega_2; + + ExtractVelocity(vertex_console_.Pose(1), vertex_console_.Pose(2), dt_2, vx_2, vy_2, omega_2); + acc_x = (vx_2 - vx) / dt; + acc_y = (vy_2 - vy) / dt; + acc_omega = (omega_2 - omega) / dt; + return true; +} + +void TebOptimal::ExtractVelocity(const DataBase &pose1, + const DataBase &pose2, + double dt, + double &vx, + double &vy, + double &omega) const { + if (dt == 0) { + vx = 0; + vy = 0; + omega = 0; + return; + } + + Eigen::Vector2d deltaS = pose2.GetPosition() - pose1.GetPosition(); + if (robot_info_.max_vel_y() == 0) { + Eigen::Vector2d conf1dir( cos(pose1.GetTheta()), sin(pose1.GetTheta()) ); + double dir = deltaS.dot(conf1dir); + vx = (double) g2o::sign(dir) * deltaS.norm()/dt; + vy = 0; + } else { + double cos_theta1 = std::cos(pose1.GetTheta()); + double sin_theta1 = std::sin(pose1.GetTheta()); + double p1_dx = cos_theta1*deltaS.coeffRef(0) + sin_theta1*deltaS.coeffRef(1); + double p1_dy = -sin_theta1*deltaS.coeffRef(0) + cos_theta1*deltaS.coeffRef(1); + vx = p1_dx / dt; + vy = p1_dy / dt; + + } + double orientdiff = g2o::normalize_theta(pose2.GetTheta() - pose1.GetTheta()); + omega = orientdiff/dt; +} + +} // namespace roborts_local_planner diff --git a/roborts_planning/local_planner/timed_elastic_band/src/teb_vertex_console.cpp b/roborts_planning/local_planner/timed_elastic_band/src/teb_vertex_console.cpp new file mode 100644 index 00000000..748b72e4 --- /dev/null +++ b/roborts_planning/local_planner/timed_elastic_band/src/teb_vertex_console.cpp @@ -0,0 +1,541 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Christoph Rösmann + *********************************************************************/ + +#include "timed_elastic_band/teb_vertex_console.h" + + +namespace roborts_local_planner { + +TebVertexConsole::TebVertexConsole() { +} + +TebVertexConsole::~TebVertexConsole() { + ClearAllVertex(); +} + +void TebVertexConsole::AddPose(const DataBase& pose, bool fixed) { + TebVertexPose *pose_vertex = new TebVertexPose(pose, fixed); + pose_vec_.push_back(pose_vertex); + return; +} + +void TebVertexConsole::AddPose(const Eigen::Ref &position, double theta, bool fixed){ + auto a = DataBase(position, theta); + TebVertexPose *pose_vertex = new TebVertexPose(a, fixed); + pose_vec_.push_back(pose_vertex); + return; +} + +void TebVertexConsole::AddTimeDiff(double dt, bool fixed) { + TebVertexTimeDiff *timediff_vertex = new TebVertexTimeDiff(dt, fixed); + timediff_vec_.push_back(timediff_vertex); + return; +} + +void TebVertexConsole::AddPoseAndTimeDiff(const DataBase &pose, double dt){ + if (SizePoses() != SizeTimeDiffs()) { + AddPose(pose, false); + AddTimeDiff(dt, false); + } else { + + } + + return; +} + +void TebVertexConsole::AddPoseAndTimeDiff(const Eigen::Ref &position, double theta, double dt){ + if (SizePoses() != SizeTimeDiffs()) { + AddPose(position, theta, false); + AddTimeDiff(dt, false); + } else { + + } + return; +} + +void TebVertexConsole::DeletePose(int index){ + delete pose_vec_.at(index); + pose_vec_.erase(pose_vec_.begin() + index); +} + +void TebVertexConsole::DeletePose(int index, int number) { + for (int i = index; i < index + number; ++i) { + delete pose_vec_.at(i); + } + pose_vec_.erase(pose_vec_.begin() + index, pose_vec_.begin() + index + number); +} + +void TebVertexConsole::DeleteTimeDiff(int index) { + delete timediff_vec_.at(index); + timediff_vec_.erase(timediff_vec_.begin() + index); +} + +void TebVertexConsole::DeleteTimeDiff(int index, int number) { + for (int i = index; i < index + number; ++i) { + delete timediff_vec_.at(i); + } + timediff_vec_.erase(timediff_vec_.begin() + index, timediff_vec_.begin() + index + number); +} + +void TebVertexConsole::InsertPose(int index, const DataBase &pose) { + TebVertexPose *pose_vertex = new TebVertexPose(pose); + pose_vec_.insert(pose_vec_.begin() + index, pose_vertex); +} + +void TebVertexConsole::InsertPose(int index, const Eigen::Ref &position, double theta) { + auto temp_data = DataBase(position, theta); + TebVertexPose *pose_vertex = new TebVertexPose(temp_data); + pose_vec_.insert(pose_vec_.begin() + index, pose_vertex); +} + +void TebVertexConsole::InsertTimeDiff(int index, double dt) { + TebVertexTimeDiff *timediff_vertex = new TebVertexTimeDiff(dt); + timediff_vec_.insert(timediff_vec_.begin() + index, timediff_vertex); +} + +void TebVertexConsole::ClearAllVertex(){ + for (PoseSequence::iterator pose_it = pose_vec_.begin(); pose_it != pose_vec_.end(); ++pose_it) { + delete *pose_it; + } + pose_vec_.clear(); + + for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it) { + delete *dt_it; + } + timediff_vec_.clear(); +} + +void TebVertexConsole::SetPoseVertexFixed(int index, bool status) { + pose_vec_.at(index)->setFixed(status); +} + +void TebVertexConsole::SetTimeDiffVertexFixed(int index, bool status) { + timediff_vec_.at(index)->setFixed(status); +} + +void TebVertexConsole::AutoResize(double dt_ref, double dt_hysteresis, int min_samples, int max_samples) { + bool modified = true; + for (int rep = 0; rep < 100 && modified; ++rep) { + modified = false; + for (int i = 0; i < SizeTimeDiffs(); ++i) { + if (TimeDiff(i) > dt_ref + dt_hysteresis && SizeTimeDiffs() < max_samples) { + double newtime = 0.5 * TimeDiff(i); + TimeDiff(i) = newtime; + DataBase temp_pose; + temp_pose.AverageInPlace(Pose(i), Pose(i + 1)); + InsertPose(i + 1, temp_pose); + InsertTimeDiff(i + 1, newtime); + modified = true; + } else if (TimeDiff(i) < dt_ref - dt_hysteresis + && SizePoses() > min_samples) { + if (i < ((int) SizeTimeDiffs() - 1)) { + TimeDiff(i + 1) = TimeDiff(i + 1) + TimeDiff(i); + DeleteTimeDiff(i); + DeletePose(i + 1); + } + modified = true; + } + } + } +} + +double TebVertexConsole::GetSumOfAllTimeDiffs() const { + double time = 0; + + for (TimeDiffSequence::const_iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it) { + time += (*dt_it)->GetDiffTime(); + } + return time; +} + +double TebVertexConsole::GetAccumulatedDistance() const { + double dist = 0; + + for (int i = 1; i < SizePoses(); ++i) { + dist += (Pose(i).GetPosition() - Pose(i - 1).GetPosition()).norm(); + } + return dist; +} + +bool TebVertexConsole::InitTEBtoGoal(const DataBase &start, + const DataBase &goal, + double diststep, + double max_vel_x, + int min_samples, + bool guess_backwards_motion) { + if (!IsInit()) { + AddPose(start); + SetPoseVertexFixed(0, true); + + double timestep = 0.1; + if (diststep != 0) { + Eigen::Vector2d point_to_goal = goal.GetPosition() - start.GetPosition(); + double dir_to_goal = std::atan2(point_to_goal.coeffRef(1), point_to_goal.coeffRef(0)); + double dx = diststep * std::cos(dir_to_goal); + double dy = diststep * std::sin(dir_to_goal); + double orient_init = dir_to_goal; + + if (guess_backwards_motion && point_to_goal.dot(start.OrientationUnitVec()) < 0) { + //orient_init = g2o::normalize_theta(orient_init + M_PI); + } + + double dist_to_goal = point_to_goal.norm(); + double no_steps_d = dist_to_goal / std::abs(diststep); + unsigned int no_steps = (unsigned int) std::floor(no_steps_d); + + if (max_vel_x > 0) { + timestep = diststep / max_vel_x; + } + + for (unsigned int i = 1; i <= no_steps; i++) { + if (i == no_steps && no_steps_d == (float) no_steps) { + break; + } + + auto temp_data = DataConverter::LocalConvertCData(start.GetPosition().coeffRef(0) + i*dx, + start.GetPosition().coeffRef(1) + i*dy, + orient_init); + AddPoseAndTimeDiff(temp_data.first, temp_data.second, timestep); + } + + } + + + if (SizePoses() < min_samples - 1) { + + while (SizePoses() < min_samples - 1) { + + + DataBase temp_pose; + temp_pose.AverageInPlace(BackPose(), goal); + if (max_vel_x > 0) { + timestep = (temp_pose.GetPosition() - goal.GetPosition()).norm() / max_vel_x; + } + AddPoseAndTimeDiff(temp_pose, timestep); + } + } + + if (max_vel_x > 0) { + timestep = (goal.GetPosition() - BackPose().GetPosition()).norm() / max_vel_x; + } + AddPoseAndTimeDiff(goal, timestep); + SetPoseVertexFixed(SizePoses() - 1, true); + } else { + + return false; + } + return true; +} + +bool TebVertexConsole::InitTEBtoGoal(std::vector &plan, + double dt, + bool estimate_orient, + int min_samples, + bool guess_backwards_motion, + bool micro_control) { + + if (!IsInit()) { + DataBase start = plan.front(); + DataBase goal = plan.back(); + + AddPose(start); + SetPoseVertexFixed(0, true); + + bool backwards = false; + /*if (guess_backwards_motion + && (goal.GetPosition() - start.GetPosition()).dot(start.OrientationUnitVec()) < 0) { + backwards = true; + }*/ + + for (int i = 1; i < (int) plan.size() - 1; ++i) { + double yaw; + if (estimate_orient && !micro_control) { + double dx = plan[i + 1].GetPosition().coeffRef(0) - plan[i].GetPosition().coeffRef(0); + double dy = plan[i + 1].GetPosition().coeffRef(1) - plan[i].GetPosition().coeffRef(1); + /*if (backwards) { + yaw = g2o::normalize_theta(std::atan2(dy, dx) + M_PI); + plan[i].SetTheta(yaw); + } else*/ { + plan[i].SetTheta(std::atan2(dy, dx)); + } + + } + AddPoseAndTimeDiff(plan[i], dt); + } + + if (SizePoses() < min_samples - 1) { + while (SizePoses() < min_samples - 1) { + DataBase temp_pose; + temp_pose.AverageInPlace(BackPose(), goal); + AddPoseAndTimeDiff(temp_pose, dt); + } + } + + + AddPoseAndTimeDiff(goal, dt); + SetPoseVertexFixed(SizePoses() - 1, true); + } else { + return false; + } + + return true; +} + +int TebVertexConsole::FindClosestTrajectoryPose(const Eigen::Ref &ref_point, + double *distance, + int begin_idx) const { + std::vector dist_vec; + dist_vec.reserve(SizePoses()); + + int n = SizePoses(); + + for (int i = begin_idx; i < n; i++) { + Eigen::Vector2d diff = ref_point - Pose(i).GetPosition(); + dist_vec.push_back(diff.norm()); + } + + if (dist_vec.empty()) + return -1; + + int index_min = 0; + + double last_value = dist_vec.at(0); + for (int i = 1; i < (int) dist_vec.size(); i++) { + if (dist_vec.at(i) < last_value) { + last_value = dist_vec.at(i); + index_min = i; + } + } + if (distance) { + *distance = last_value; + } + return begin_idx + index_min; +} + +int TebVertexConsole::FindClosestTrajectoryPose(const Eigen::Ref &ref_line_start, + const Eigen::Ref &ref_line_end, + double *distance) const { + std::vector dist_vec; + dist_vec.reserve(SizePoses()); + + int n = SizePoses(); + + + for (int i = 0; i < n; i++) { + Eigen::Vector2d point = Pose(i).GetPosition(); + double diff = DistancePointToSegment2D(point, ref_line_start, ref_line_end); + dist_vec.push_back(diff); + } + + if (dist_vec.empty()) { + return -1; + } + + int index_min = 0; + + double last_value = dist_vec.at(0); + for (int i = 1; i < (int) dist_vec.size(); i++) { + if (dist_vec.at(i) < last_value) { + last_value = dist_vec.at(i); + index_min = i; + } + } + if (distance) { + *distance = last_value; + } + + return index_min; +} + +int TebVertexConsole::FindClosestTrajectoryPose(const Point2dContainer &vertices, double *distance) const { + if (vertices.empty()) { + return 0; + } else if (vertices.size() == 1) { + return FindClosestTrajectoryPose(vertices.front()); + } else if (vertices.size() == 2) { + return FindClosestTrajectoryPose(vertices.front(), vertices.back()); + } + + std::vector dist_vec; + dist_vec.reserve(SizePoses()); + + int n = SizePoses(); + + for (int i = 0; i < n; i++) { + Eigen::Vector2d point = Pose(i).GetPosition(); + double diff = HUGE_VAL; + for (int j = 0; j < (int) vertices.size() - 1; ++j) { + diff = std::min(diff, DistancePointToSegment2D(point, vertices[j], vertices[j + 1])); + } + diff = std::min(diff, DistancePointToSegment2D(point, vertices.back(), vertices.front())); + dist_vec.push_back(diff); + } + + if (dist_vec.empty()) { + return -1; + } + + int index_min = 0; + + double last_value = dist_vec.at(0); + for (int i = 1; i < (int) dist_vec.size(); i++) { + if (dist_vec.at(i) < last_value) { + last_value = dist_vec.at(i); + index_min = i; + } + } + if (distance) { + *distance = last_value; + } + + return index_min; +} + +int TebVertexConsole::FindClosestTrajectoryPose(const Obstacle &obstacle, double *distance) const { + const PointObstacle *pobst = dynamic_cast(&obstacle); + if (pobst) { + return FindClosestTrajectoryPose(pobst->Position(), distance); + } + + const LineObstacle *lobst = dynamic_cast(&obstacle); + if (lobst) { + return FindClosestTrajectoryPose(lobst->Start(), lobst->End(), distance); + } + + const PolygonObstacle *polyobst = dynamic_cast(&obstacle); + if (polyobst) { + return FindClosestTrajectoryPose(polyobst->Vertices(), distance); + } + + return FindClosestTrajectoryPose(obstacle.GetCentroid(), distance); +} + +bool TebVertexConsole::DetectDetoursBackwards(double threshold) const { + if (SizePoses() < 2) { + return false; + } + + Eigen::Vector2d d_start_goal = BackPose().GetPosition() - Pose(0).GetPosition(); + d_start_goal.normalize(); + + for (int i = 0; i < SizePoses(); ++i) { + Eigen::Vector2d orient_vector(cos(Pose(i).GetTheta()), sin(Pose(i).GetTheta())); + if (orient_vector.dot(d_start_goal) < threshold) { + + return true; + } + } + + return false; +} + +void TebVertexConsole::UpdateAndPruneTEB(boost::optional new_start, + boost::optional new_goal, + int min_samples){ + + if (new_start && SizePoses() > 0) { + double dist_cache = (new_start->GetPosition() - Pose(0).GetPosition()).norm(); + double dist; + int lookahead = std::min(SizePoses() - min_samples, 10); + int nearest_idx = 0; + for (int i = 1; i <= lookahead; ++i) { + dist = (new_start->GetPosition() - Pose(i).GetPosition()).norm(); + if (dist < dist_cache) { + dist_cache = dist; + nearest_idx = i; + } else { + break; + } + } + + if (nearest_idx > 0) { + DeletePose(1, nearest_idx); + DeleteTimeDiff(1, nearest_idx); + } + + Pose(0) = *new_start; + } + + if (new_goal && SizePoses() > 0) { + BackPose() = *new_goal; + } +} + +bool TebVertexConsole::IsTrajectoryInsideRegion(double radius, double max_dist_behind_robot, int skip_poses) { + if (SizePoses() <= 0){ + return true; + } + + double radius_sq = radius * radius; + double max_dist_behind_robot_sq = max_dist_behind_robot * max_dist_behind_robot; + Eigen::Vector2d robot_orient = Pose(0).OrientationUnitVec(); + + for (int i = 1; i < SizePoses(); i = i + skip_poses + 1) { + Eigen::Vector2d dist_vec = Pose(i).GetPosition() - Pose(0).GetPosition(); + double dist_sq = dist_vec.squaredNorm(); + + if (dist_sq > radius_sq) { + return false; + } + + if (max_dist_behind_robot >= 0 && dist_vec.dot(robot_orient) < 0 && dist_sq > max_dist_behind_robot_sq) { + return false; + } + + } + return true; +} + +} // namespace roborts_local_planner \ No newline at end of file diff --git a/roborts_planning/package.xml b/roborts_planning/package.xml new file mode 100755 index 00000000..15255ec6 --- /dev/null +++ b/roborts_planning/package.xml @@ -0,0 +1,31 @@ + + roborts_planning + 1.0.0 + + The roborts planning package provides motion planning for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + actionlib + roscpp + roborts_msgs + rospy + roborts_common + roborts_costmap + + actionlib + roscpp + roborts_msgs + rospy + roborts_common + roborts_costmap + + + + + diff --git a/roborts_tracking/CMakeLists.txt b/roborts_tracking/CMakeLists.txt new file mode 100644 index 00000000..969c19c7 --- /dev/null +++ b/roborts_tracking/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.0) +project(roborts_tracking) +set(CMAKE_BUILD_TYPE Release) +find_package(OpenCV REQUIRED) +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + roborts_msgs + ) + +if(NOT WIN32) + ADD_DEFINITIONS("-std=c++11") +endif(NOT WIN32) + +catkin_package() + +add_library(kcf_tracker STATIC + KCFcpp/src/fhog.cpp + KCFcpp/src/kcftracker.cpp) +target_link_libraries(kcf_tracker ${OpenCV_LIBRARIES}) +target_include_directories(kcf_tracker PRIVATE KCFcpp/src) + +add_executable(roborts_tracking_test + tracking_test.cpp + tracking_utility.cpp) +target_link_libraries(roborts_tracking_test + kcf_tracker + ${OpenCV_LIBS} + ${catkin_LIBRARIES}) +target_include_directories(roborts_tracking_test + PUBLIC + ${catkin_INCLUDE_DIRS}) + + diff --git a/roborts_tracking/KCFcpp/CMakeLists.txt b/roborts_tracking/KCFcpp/CMakeLists.txt new file mode 100644 index 00000000..bf1c0655 --- /dev/null +++ b/roborts_tracking/KCFcpp/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 2.8) + +project(test) + +find_package(OpenCV REQUIRED) + +if(NOT WIN32) +ADD_DEFINITIONS("-std=c++0x -O3") +endif(NOT WIN32) + +include_directories(src) +FILE(GLOB_RECURSE sourcefiles "src/*.cpp") +add_executable( KCF ${sourcefiles} ) +target_link_libraries( KCF ${OpenCV_LIBS}) + + + + diff --git a/roborts_tracking/KCFcpp/KCFCpp.sh b/roborts_tracking/KCFcpp/KCFCpp.sh new file mode 100644 index 00000000..a42afd51 --- /dev/null +++ b/roborts_tracking/KCFcpp/KCFCpp.sh @@ -0,0 +1,3 @@ + +KCF + diff --git a/roborts_tracking/KCFcpp/KCFLabCpp.sh b/roborts_tracking/KCFcpp/KCFLabCpp.sh new file mode 100644 index 00000000..2e2e6d29 --- /dev/null +++ b/roborts_tracking/KCFcpp/KCFLabCpp.sh @@ -0,0 +1,3 @@ + +KCF lab + diff --git a/roborts_tracking/KCFcpp/LICENSE b/roborts_tracking/KCFcpp/LICENSE new file mode 100644 index 00000000..d9c1b318 --- /dev/null +++ b/roborts_tracking/KCFcpp/LICENSE @@ -0,0 +1,28 @@ +Copyright (c) 2015, Joao Faro +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of KCFcpp nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/roborts_tracking/KCFcpp/README.md b/roborts_tracking/KCFcpp/README.md new file mode 100644 index 00000000..26e748a8 --- /dev/null +++ b/roborts_tracking/KCFcpp/README.md @@ -0,0 +1,56 @@ +# C++ KCF Tracker +This package includes a C++ class with several tracking methods based on the Kernelized Correlation Filter (KCF) [1, 2]. +It also includes an executable to interface with the VOT benchmark. + +[1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, +"High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015. + +[2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, +"Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012. + + +Authors: Joao Faro, Christian Bailer, Joao F. Henriques +Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt +Institute of Systems and Robotics - University of Coimbra / Department of Augmented Vision DFKI + +### Algorithms (in this folder) ### + +"KCFC++", command: ./KCF +Description: KCF on HOG features, ported to C++ OpenCV. The original Matlab tracker placed 3rd in VOT 2014. + +"KCFLabC++", command: ./KCF lab +Description: KCF on HOG and Lab features, ported to C++ OpenCV. The Lab features are computed by quantizing CIE-Lab colors into 15 centroids, obtained from natural images by k-means. + +The CSK tracker [2] is also implemented as a bonus, simply by using raw grayscale as features (the filter becomes single-channel). + +### Compilation instructions ### +There are no external dependencies other than OpenCV 3.0.0. Tested on a freshly installed Ubuntu 14.04. + +1) cmake CMakeLists.txt +2) make + +### Running instructions ### + +The runtracker.cpp is prepared to be used with the VOT toolkit. The executable "KCF" should be called as: + +./KCF [OPTION_1] [OPTION_2] [...] + +Options available: + +gray - Use raw gray level features as in [1]. +hog - Use HOG features as in [2]. +lab - Use Lab colorspace features. This option will also enable HOG features by default. +singlescale - Performs single-scale detection, using a variable-size window. +fixed_window - Keep the window size fixed when in single-scale mode (multi-scale always used a fixed window). +show - Show the results in a window. + +To include it in your project, without the VOT toolkit you just need to: + + // Create the KCFTracker object with one of the available options + KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB); + + // Give the first frame and the position of the object to the tracker + tracker.init( Rect(xMin, yMin, width, height), frame ); + + // Get the position of the object for the new frame + result = tracker.update(frame); diff --git a/roborts_tracking/KCFcpp/src/ffttools.hpp b/roborts_tracking/KCFcpp/src/ffttools.hpp new file mode 100644 index 00000000..8580a27c --- /dev/null +++ b/roborts_tracking/KCFcpp/src/ffttools.hpp @@ -0,0 +1,237 @@ +/* +Author: Christian Bailer +Contact address: Christian.Bailer@dfki.de +Department Augmented Vision DFKI + + License Agreement + For Open Source Computer Vision Library + (3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the names of the copyright holders nor the names of the contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +This software is provided by the copyright holders and contributors "as is" and +any express or implied warranties, including, but not limited to, the implied +warranties of merchantability and fitness for a particular purpose are disclaimed. +In no event shall copyright holders or contributors be liable for any direct, +indirect, incidental, special, exemplary, or consequential damages +(including, but not limited to, procurement of substitute goods or services; +loss of use, data, or profits; or business interruption) however caused +and on any theory of liability, whether in contract, strict liability, +or tort (including negligence or otherwise) arising in any way out of +the use of this software, even if advised of the possibility of such damage. +*/ + +#pragma once + +//#include + +#ifndef _OPENCV_FFTTOOLS_HPP_ +#define _OPENCV_FFTTOOLS_HPP_ +#endif + +//NOTE: FFTW support is still shaky, disabled for now. +/*#ifdef USE_FFTW +#include +#endif*/ + +namespace FFTTools +{ +// Previous declarations, to avoid warnings +cv::Mat fftd(cv::Mat img, bool backwards = false); +cv::Mat real(cv::Mat img); +cv::Mat imag(cv::Mat img); +cv::Mat magnitude(cv::Mat img); +cv::Mat complexMultiplication(cv::Mat a, cv::Mat b); +cv::Mat complexDivision(cv::Mat a, cv::Mat b); +void rearrange(cv::Mat &img); +void normalizedLogTransform(cv::Mat &img); + + +cv::Mat fftd(cv::Mat img, bool backwards) +{ +/* +#ifdef USE_FFTW + + fftw_complex * fm = (fftw_complex*) fftw_malloc(sizeof (fftw_complex) * img.cols * img.rows); + + fftw_plan p = fftw_plan_dft_2d(img.rows, img.cols, fm, fm, backwards ? 1 : -1, 0 * FFTW_ESTIMATE); + + + if (img.channels() == 1) + { + for (int i = 0; i < img.rows; i++) + for (int j = 0; j < img.cols; j++) + { + fm[i * img.cols + j][0] = img.at(i, j); + fm[i * img.cols + j][1] = 0; + } + } + else + { + assert(img.channels() == 2); + for (int i = 0; i < img.rows; i++) + for (int j = 0; j < img.cols; j++) + { + fm[i * img.cols + j][0] = img.at (i, j)[0]; + fm[i * img.cols + j][1] = img.at (i, j)[1]; + } + } + fftw_execute(p); + cv::Mat res(img.rows, img.cols, CV_64FC2); + + + for (int i = 0; i < img.rows; i++) + for (int j = 0; j < img.cols; j++) + { + res.at (i, j)[0] = fm[i * img.cols + j][0]; + res.at (i, j)[1] = fm[i * img.cols + j][1]; + + // _iout(fm[i * img.cols + j][0]); + } + + if (backwards)res *= 1.d / (float) (res.cols * res.rows); + + fftw_free(p); + fftw_free(fm); + return res; + +#else +*/ + if (img.channels() == 1) + { + cv::Mat planes[] = {cv::Mat_ (img), cv::Mat_::zeros(img.size())}; + //cv::Mat planes[] = {cv::Mat_ (img), cv::Mat_::zeros(img.size())}; + cv::merge(planes, 2, img); + } + cv::dft(img, img, backwards ? (cv::DFT_INVERSE | cv::DFT_SCALE) : 0 ); + + return img; + +/*#endif*/ + +} + +cv::Mat real(cv::Mat img) +{ + std::vector planes; + cv::split(img, planes); + return planes[0]; +} + +cv::Mat imag(cv::Mat img) +{ + std::vector planes; + cv::split(img, planes); + return planes[1]; +} + +cv::Mat magnitude(cv::Mat img) +{ + cv::Mat res; + std::vector planes; + cv::split(img, planes); // planes[0] = Re(DFT(I), planes[1] = Im(DFT(I)) + if (planes.size() == 1) res = cv::abs(img); + else if (planes.size() == 2) cv::magnitude(planes[0], planes[1], res); // planes[0] = magnitude + else assert(0); + return res; +} + +cv::Mat complexMultiplication(cv::Mat a, cv::Mat b) +{ + std::vector pa; + std::vector pb; + cv::split(a, pa); + cv::split(b, pb); + + std::vector pres; + pres.push_back(pa[0].mul(pb[0]) - pa[1].mul(pb[1])); + pres.push_back(pa[0].mul(pb[1]) + pa[1].mul(pb[0])); + + cv::Mat res; + cv::merge(pres, res); + + return res; +} + +cv::Mat complexDivision(cv::Mat a, cv::Mat b) +{ + std::vector pa; + std::vector pb; + cv::split(a, pa); + cv::split(b, pb); + + cv::Mat divisor = 1. / (pb[0].mul(pb[0]) + pb[1].mul(pb[1])); + + std::vector pres; + + pres.push_back((pa[0].mul(pb[0]) + pa[1].mul(pb[1])).mul(divisor)); + pres.push_back((pa[1].mul(pb[0]) + pa[0].mul(pb[1])).mul(divisor)); + + cv::Mat res; + cv::merge(pres, res); + return res; +} + +void rearrange(cv::Mat &img) +{ + // img = img(cv::Rect(0, 0, img.cols & -2, img.rows & -2)); + int cx = img.cols / 2; + int cy = img.rows / 2; + + cv::Mat q0(img, cv::Rect(0, 0, cx, cy)); // Top-Left - Create a ROI per quadrant + cv::Mat q1(img, cv::Rect(cx, 0, cx, cy)); // Top-Right + cv::Mat q2(img, cv::Rect(0, cy, cx, cy)); // Bottom-Left + cv::Mat q3(img, cv::Rect(cx, cy, cx, cy)); // Bottom-Right + + cv::Mat tmp; // swap quadrants (Top-Left with Bottom-Right) + q0.copyTo(tmp); + q3.copyTo(q0); + tmp.copyTo(q3); + q1.copyTo(tmp); // swap quadrant (Top-Right with Bottom-Left) + q2.copyTo(q1); + tmp.copyTo(q2); +} +/* +template < typename type> +cv::Mat fouriertransFull(const cv::Mat & in) +{ + return fftd(in); + + cv::Mat planes[] = {cv::Mat_ (in), cv::Mat_::zeros(in.size())}; + cv::Mat t; + assert(planes[0].depth() == planes[1].depth()); + assert(planes[0].size == planes[1].size); + cv::merge(planes, 2, t); + cv::dft(t, t); + + //cv::normalize(a, a, 0, 1, CV_MINMAX); + //cv::normalize(t, t, 0, 1, CV_MINMAX); + + // cv::imshow("a",real(a)); + // cv::imshow("b",real(t)); + // cv::waitKey(0); + + return t; +}*/ + +void normalizedLogTransform(cv::Mat &img) +{ + img = cv::abs(img); + img += cv::Scalar::all(1); + cv::log(img, img); + // cv::normalize(img, img, 0, 1, CV_MINMAX); +} + +} diff --git a/roborts_tracking/KCFcpp/src/fhog.cpp b/roborts_tracking/KCFcpp/src/fhog.cpp new file mode 100644 index 00000000..28898e83 --- /dev/null +++ b/roborts_tracking/KCFcpp/src/fhog.cpp @@ -0,0 +1,511 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2010-2013, University of Nizhny Novgorod, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + + +//Modified from latentsvm module's "lsvmc_featurepyramid.cpp". + +//#include "precomp.hpp" +//#include "_lsvmc_latentsvm.h" +//#include "_lsvmc_resizeimg.h" + +#include "fhog.hpp" + + +#ifdef HAVE_TBB +#include +#include "tbb/parallel_for.h" +#include "tbb/blocked_range.h" +#endif + +#ifndef max +#define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif + +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + + +/* +// Getting feature map for the selected subimage +// +// API +// int getFeatureMaps(const IplImage * image, const int k, featureMap **map); +// INPUT +// image - selected subimage +// k - size of cells +// OUTPUT +// map - feature map +// RESULT +// Error status +*/ +int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade **map) +{ + int sizeX, sizeY; + int p, px, stringSize; + int height, width, numChannels; + int i, j, kk, c, ii, jj, d; + float * datadx, * datady; + + int ch; + float magnitude, x, y, tx, ty; + + IplImage * dx, * dy; + int *nearest; + float *w, a_x, b_x; + + float kernel[3] = {-1.f, 0.f, 1.f}; + CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel); + CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel); + + float * r; + int * alfa; + + float boundary_x[NUM_SECTOR + 1]; + float boundary_y[NUM_SECTOR + 1]; + float max, dotProd; + int maxi; + + height = image->height; + width = image->width ; + + numChannels = image->nChannels; + + dx = cvCreateImage(cvSize(image->width, image->height), + IPL_DEPTH_32F, 3); + dy = cvCreateImage(cvSize(image->width, image->height), + IPL_DEPTH_32F, 3); + + sizeX = width / k; + sizeY = height / k; + px = 3 * NUM_SECTOR; + p = px; + stringSize = sizeX * p; + allocFeatureMapObject(map, sizeX, sizeY, p); + + cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0)); + cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1)); + + float arg_vector; + for(i = 0; i <= NUM_SECTOR; i++) + { + arg_vector = ( (float) i ) * ( (float)(PI) / (float)(NUM_SECTOR) ); + boundary_x[i] = cosf(arg_vector); + boundary_y[i] = sinf(arg_vector); + }/*for(i = 0; i <= NUM_SECTOR; i++) */ + + r = (float *)malloc( sizeof(float) * (width * height)); + alfa = (int *)malloc( sizeof(int ) * (width * height * 2)); + + for(j = 1; j < height - 1; j++) + { + datadx = (float*)(dx->imageData + dx->widthStep * j); + datady = (float*)(dy->imageData + dy->widthStep * j); + for(i = 1; i < width - 1; i++) + { + c = 0; + x = (datadx[i * numChannels + c]); + y = (datady[i * numChannels + c]); + + r[j * width + i] =sqrtf(x * x + y * y); + for(ch = 1; ch < numChannels; ch++) + { + tx = (datadx[i * numChannels + ch]); + ty = (datady[i * numChannels + ch]); + magnitude = sqrtf(tx * tx + ty * ty); + if(magnitude > r[j * width + i]) + { + r[j * width + i] = magnitude; + c = ch; + x = tx; + y = ty; + } + }/*for(ch = 1; ch < numChannels; ch++)*/ + + max = boundary_x[0] * x + boundary_y[0] * y; + maxi = 0; + for (kk = 0; kk < NUM_SECTOR; kk++) + { + dotProd = boundary_x[kk] * x + boundary_y[kk] * y; + if (dotProd > max) + { + max = dotProd; + maxi = kk; + } + else + { + if (-dotProd > max) + { + max = -dotProd; + maxi = kk + NUM_SECTOR; + } + } + } + alfa[j * width * 2 + i * 2 ] = maxi % NUM_SECTOR; + alfa[j * width * 2 + i * 2 + 1] = maxi; + }/*for(i = 0; i < width; i++)*/ + }/*for(j = 0; j < height; j++)*/ + + nearest = (int *)malloc(sizeof(int ) * k); + w = (float*)malloc(sizeof(float) * (k * 2)); + + for(i = 0; i < k / 2; i++) + { + nearest[i] = -1; + }/*for(i = 0; i < k / 2; i++)*/ + for(i = k / 2; i < k; i++) + { + nearest[i] = 1; + }/*for(i = k / 2; i < k; i++)*/ + + for(j = 0; j < k / 2; j++) + { + b_x = k / 2 + j + 0.5f; + a_x = k / 2 - j - 0.5f; + w[j * 2 ] = 1.0f/a_x * ((a_x * b_x) / ( a_x + b_x)); + w[j * 2 + 1] = 1.0f/b_x * ((a_x * b_x) / ( a_x + b_x)); + }/*for(j = 0; j < k / 2; j++)*/ + for(j = k / 2; j < k; j++) + { + a_x = j - k / 2 + 0.5f; + b_x =-j + k / 2 - 0.5f + k; + w[j * 2 ] = 1.0f/a_x * ((a_x * b_x) / ( a_x + b_x)); + w[j * 2 + 1] = 1.0f/b_x * ((a_x * b_x) / ( a_x + b_x)); + }/*for(j = k / 2; j < k; j++)*/ + + for(i = 0; i < sizeY; i++) + { + for(j = 0; j < sizeX; j++) + { + for(ii = 0; ii < k; ii++) + { + for(jj = 0; jj < k; jj++) + { + if ((i * k + ii > 0) && + (i * k + ii < height - 1) && + (j * k + jj > 0) && + (j * k + jj < width - 1)) + { + d = (k * i + ii) * width + (j * k + jj); + (*map)->map[ i * stringSize + j * (*map)->numFeatures + alfa[d * 2 ]] += + r[d] * w[ii * 2] * w[jj * 2]; + (*map)->map[ i * stringSize + j * (*map)->numFeatures + alfa[d * 2 + 1] + NUM_SECTOR] += + r[d] * w[ii * 2] * w[jj * 2]; + if ((i + nearest[ii] >= 0) && + (i + nearest[ii] <= sizeY - 1)) + { + (*map)->map[(i + nearest[ii]) * stringSize + j * (*map)->numFeatures + alfa[d * 2 ] ] += + r[d] * w[ii * 2 + 1] * w[jj * 2 ]; + (*map)->map[(i + nearest[ii]) * stringSize + j * (*map)->numFeatures + alfa[d * 2 + 1] + NUM_SECTOR] += + r[d] * w[ii * 2 + 1] * w[jj * 2 ]; + } + if ((j + nearest[jj] >= 0) && + (j + nearest[jj] <= sizeX - 1)) + { + (*map)->map[i * stringSize + (j + nearest[jj]) * (*map)->numFeatures + alfa[d * 2 ] ] += + r[d] * w[ii * 2] * w[jj * 2 + 1]; + (*map)->map[i * stringSize + (j + nearest[jj]) * (*map)->numFeatures + alfa[d * 2 + 1] + NUM_SECTOR] += + r[d] * w[ii * 2] * w[jj * 2 + 1]; + } + if ((i + nearest[ii] >= 0) && + (i + nearest[ii] <= sizeY - 1) && + (j + nearest[jj] >= 0) && + (j + nearest[jj] <= sizeX - 1)) + { + (*map)->map[(i + nearest[ii]) * stringSize + (j + nearest[jj]) * (*map)->numFeatures + alfa[d * 2 ] ] += + r[d] * w[ii * 2 + 1] * w[jj * 2 + 1]; + (*map)->map[(i + nearest[ii]) * stringSize + (j + nearest[jj]) * (*map)->numFeatures + alfa[d * 2 + 1] + NUM_SECTOR] += + r[d] * w[ii * 2 + 1] * w[jj * 2 + 1]; + } + } + }/*for(jj = 0; jj < k; jj++)*/ + }/*for(ii = 0; ii < k; ii++)*/ + }/*for(j = 1; j < sizeX - 1; j++)*/ + }/*for(i = 1; i < sizeY - 1; i++)*/ + + cvReleaseImage(&dx); + cvReleaseImage(&dy); + + + free(w); + free(nearest); + + free(r); + free(alfa); + + return LATENT_SVM_OK; +} + +/* +// Feature map Normalization and Truncation +// +// API +// int normalizeAndTruncate(featureMap *map, const float alfa); +// INPUT +// map - feature map +// alfa - truncation threshold +// OUTPUT +// map - truncated and normalized feature map +// RESULT +// Error status +*/ +int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa) +{ + int i,j, ii; + int sizeX, sizeY, p, pos, pp, xp, pos1, pos2; + float * partOfNorm; // norm of C(i, j) + float * newData; + float valOfNorm; + + sizeX = map->sizeX; + sizeY = map->sizeY; + partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); + + p = NUM_SECTOR; + xp = NUM_SECTOR * 3; + pp = NUM_SECTOR * 12; + + for(i = 0; i < sizeX * sizeY; i++) + { + valOfNorm = 0.0f; + pos = i * map->numFeatures; + for(j = 0; j < p; j++) + { + valOfNorm += map->map[pos + j] * map->map[pos + j]; + }/*for(j = 0; j < p; j++)*/ + partOfNorm[i] = valOfNorm; + }/*for(i = 0; i < sizeX * sizeY; i++)*/ + + sizeX -= 2; + sizeY -= 2; + + newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); +//normalization + for(i = 1; i <= sizeY; i++) + { + for(j = 1; j <= sizeX; j++) + { + valOfNorm = sqrtf( + partOfNorm[(i )*(sizeX + 2) + (j )] + + partOfNorm[(i )*(sizeX + 2) + (j + 1)] + + partOfNorm[(i + 1)*(sizeX + 2) + (j )] + + partOfNorm[(i + 1)*(sizeX + 2) + (j + 1)]) + FLT_EPSILON; + pos1 = (i ) * (sizeX + 2) * xp + (j ) * xp; + pos2 = (i-1) * (sizeX ) * pp + (j-1) * pp; + for(ii = 0; ii < p; ii++) + { + newData[pos2 + ii ] = map->map[pos1 + ii ] / valOfNorm; + }/*for(ii = 0; ii < p; ii++)*/ + for(ii = 0; ii < 2 * p; ii++) + { + newData[pos2 + ii + p * 4] = map->map[pos1 + ii + p] / valOfNorm; + }/*for(ii = 0; ii < 2 * p; ii++)*/ + valOfNorm = sqrtf( + partOfNorm[(i )*(sizeX + 2) + (j )] + + partOfNorm[(i )*(sizeX + 2) + (j + 1)] + + partOfNorm[(i - 1)*(sizeX + 2) + (j )] + + partOfNorm[(i - 1)*(sizeX + 2) + (j + 1)]) + FLT_EPSILON; + for(ii = 0; ii < p; ii++) + { + newData[pos2 + ii + p ] = map->map[pos1 + ii ] / valOfNorm; + }/*for(ii = 0; ii < p; ii++)*/ + for(ii = 0; ii < 2 * p; ii++) + { + newData[pos2 + ii + p * 6] = map->map[pos1 + ii + p] / valOfNorm; + }/*for(ii = 0; ii < 2 * p; ii++)*/ + valOfNorm = sqrtf( + partOfNorm[(i )*(sizeX + 2) + (j )] + + partOfNorm[(i )*(sizeX + 2) + (j - 1)] + + partOfNorm[(i + 1)*(sizeX + 2) + (j )] + + partOfNorm[(i + 1)*(sizeX + 2) + (j - 1)]) + FLT_EPSILON; + for(ii = 0; ii < p; ii++) + { + newData[pos2 + ii + p * 2] = map->map[pos1 + ii ] / valOfNorm; + }/*for(ii = 0; ii < p; ii++)*/ + for(ii = 0; ii < 2 * p; ii++) + { + newData[pos2 + ii + p * 8] = map->map[pos1 + ii + p] / valOfNorm; + }/*for(ii = 0; ii < 2 * p; ii++)*/ + valOfNorm = sqrtf( + partOfNorm[(i )*(sizeX + 2) + (j )] + + partOfNorm[(i )*(sizeX + 2) + (j - 1)] + + partOfNorm[(i - 1)*(sizeX + 2) + (j )] + + partOfNorm[(i - 1)*(sizeX + 2) + (j - 1)]) + FLT_EPSILON; + for(ii = 0; ii < p; ii++) + { + newData[pos2 + ii + p * 3 ] = map->map[pos1 + ii ] / valOfNorm; + }/*for(ii = 0; ii < p; ii++)*/ + for(ii = 0; ii < 2 * p; ii++) + { + newData[pos2 + ii + p * 10] = map->map[pos1 + ii + p] / valOfNorm; + }/*for(ii = 0; ii < 2 * p; ii++)*/ + }/*for(j = 1; j <= sizeX; j++)*/ + }/*for(i = 1; i <= sizeY; i++)*/ +//truncation + for(i = 0; i < sizeX * sizeY * pp; i++) + { + if(newData [i] > alfa) newData [i] = alfa; + }/*for(i = 0; i < sizeX * sizeY * pp; i++)*/ +//swop data + + map->numFeatures = pp; + map->sizeX = sizeX; + map->sizeY = sizeY; + + free (map->map); + free (partOfNorm); + + map->map = newData; + + return LATENT_SVM_OK; +} +/* +// Feature map reduction +// In each cell we reduce dimension of the feature vector +// according to original paper special procedure +// +// API +// int PCAFeatureMaps(featureMap *map) +// INPUT +// map - feature map +// OUTPUT +// map - feature map +// RESULT +// Error status +*/ +int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map) +{ + int i,j, ii, jj, k; + int sizeX, sizeY, p, pp, xp, yp, pos1, pos2; + float * newData; + float val; + float nx, ny; + + sizeX = map->sizeX; + sizeY = map->sizeY; + p = map->numFeatures; + pp = NUM_SECTOR * 3 + 4; + yp = 4; + xp = NUM_SECTOR; + + nx = 1.0f / sqrtf((float)(xp * 2)); + ny = 1.0f / sqrtf((float)(yp )); + + newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); + + for(i = 0; i < sizeY; i++) + { + for(j = 0; j < sizeX; j++) + { + pos1 = ((i)*sizeX + j)*p; + pos2 = ((i)*sizeX + j)*pp; + k = 0; + for(jj = 0; jj < xp * 2; jj++) + { + val = 0; + for(ii = 0; ii < yp; ii++) + { + val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; + }/*for(ii = 0; ii < yp; ii++)*/ + newData[pos2 + k] = val * ny; + k++; + }/*for(jj = 0; jj < xp * 2; jj++)*/ + for(jj = 0; jj < xp; jj++) + { + val = 0; + for(ii = 0; ii < yp; ii++) + { + val += map->map[pos1 + ii * xp + jj]; + }/*for(ii = 0; ii < yp; ii++)*/ + newData[pos2 + k] = val * ny; + k++; + }/*for(jj = 0; jj < xp; jj++)*/ + for(ii = 0; ii < yp; ii++) + { + val = 0; + for(jj = 0; jj < 2 * xp; jj++) + { + val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; + }/*for(jj = 0; jj < xp; jj++)*/ + newData[pos2 + k] = val * nx; + k++; + } /*for(ii = 0; ii < yp; ii++)*/ + }/*for(j = 0; j < sizeX; j++)*/ + }/*for(i = 0; i < sizeY; i++)*/ +//swop data + + map->numFeatures = pp; + + free (map->map); + + map->map = newData; + + return LATENT_SVM_OK; +} + + +//modified from "lsvmc_routine.cpp" + +int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX, + const int sizeY, const int numFeatures) +{ + int i; + (*obj) = (CvLSVMFeatureMapCaskade *)malloc(sizeof(CvLSVMFeatureMapCaskade)); + (*obj)->sizeX = sizeX; + (*obj)->sizeY = sizeY; + (*obj)->numFeatures = numFeatures; + (*obj)->map = (float *) malloc(sizeof (float) * + (sizeX * sizeY * numFeatures)); + for(i = 0; i < sizeX * sizeY * numFeatures; i++) + { + (*obj)->map[i] = 0.0f; + } + return LATENT_SVM_OK; +} + +int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj) +{ + if(*obj == NULL) return LATENT_SVM_MEM_NULL; + free((*obj)->map); + free(*obj); + (*obj) = NULL; + return LATENT_SVM_OK; +} diff --git a/roborts_tracking/KCFcpp/src/fhog.hpp b/roborts_tracking/KCFcpp/src/fhog.hpp new file mode 100644 index 00000000..060bfec6 --- /dev/null +++ b/roborts_tracking/KCFcpp/src/fhog.hpp @@ -0,0 +1,178 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2010-2013, University of Nizhny Novgorod, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + + +//Modified from latentsvm module's "_lsvmc_latentsvm.h". + + +/*****************************************************************************/ +/* Latent SVM prediction API */ +/*****************************************************************************/ + +#ifndef _FHOG_H_ +#define _FHOG_H_ + +#include +//#include "_lsvmc_types.h" +//#include "_lsvmc_error.h" +//#include "_lsvmc_routine.h" + +//#include "opencv2/imgproc.hpp" +#include "opencv2/imgproc/imgproc_c.h" + + +//modified from "_lsvmc_types.h" + +// DataType: STRUCT featureMap +// FEATURE MAP DESCRIPTION +// Rectangular map (sizeX x sizeY), +// every cell stores feature vector (dimension = numFeatures) +// map - matrix of feature vectors +// to set and get feature vectors (i,j) +// used formula map[(j * sizeX + i) * p + k], where +// k - component of feature vector in cell (i, j) +typedef struct{ + int sizeX; + int sizeY; + int numFeatures; + float *map; +} CvLSVMFeatureMapCaskade; + + +#include "float.h" + +#define PI CV_PI + +#define EPS 0.000001 + +#define F_MAX FLT_MAX +#define F_MIN -FLT_MAX + +// The number of elements in bin +// The number of sectors in gradient histogram building +#define NUM_SECTOR 9 + +// The number of levels in image resize procedure +// We need Lambda levels to resize image twice +#define LAMBDA 10 + +// Block size. Used in feature pyramid building procedure +#define SIDE_LENGTH 8 + +#define VAL_OF_TRUNCATE 0.2f + + +//modified from "_lsvm_error.h" +#define LATENT_SVM_OK 0 +#define LATENT_SVM_MEM_NULL 2 +#define DISTANCE_TRANSFORM_OK 1 +#define DISTANCE_TRANSFORM_GET_INTERSECTION_ERROR -1 +#define DISTANCE_TRANSFORM_ERROR -2 +#define DISTANCE_TRANSFORM_EQUAL_POINTS -3 +#define LATENT_SVM_GET_FEATURE_PYRAMID_FAILED -4 +#define LATENT_SVM_SEARCH_OBJECT_FAILED -5 +#define LATENT_SVM_FAILED_SUPERPOSITION -6 +#define FILTER_OUT_OF_BOUNDARIES -7 +#define LATENT_SVM_TBB_SCHEDULE_CREATION_FAILED -8 +#define LATENT_SVM_TBB_NUMTHREADS_NOT_CORRECT -9 +#define FFT_OK 2 +#define FFT_ERROR -10 +#define LSVM_PARSER_FILE_NOT_FOUND -11 + + + +/* +// Getting feature map for the selected subimage +// +// API +// int getFeatureMaps(const IplImage * image, const int k, featureMap **map); +// INPUT +// image - selected subimage +// k - size of cells +// OUTPUT +// map - feature map +// RESULT +// Error status +*/ +int getFeatureMaps(const IplImage * image, const int k, CvLSVMFeatureMapCaskade **map); + + +/* +// Feature map Normalization and Truncation +// +// API +// int normalizationAndTruncationFeatureMaps(featureMap *map, const float alfa); +// INPUT +// map - feature map +// alfa - truncation threshold +// OUTPUT +// map - truncated and normalized feature map +// RESULT +// Error status +*/ +int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa); + +/* +// Feature map reduction +// In each cell we reduce dimension of the feature vector +// according to original paper special procedure +// +// API +// int PCAFeatureMaps(featureMap *map) +// INPUT +// map - feature map +// OUTPUT +// map - feature map +// RESULT +// Error status +*/ +int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map); + + +//modified from "lsvmc_routine.h" + +int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX, const int sizeY, + const int p); + +int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj); + + +#endif diff --git a/roborts_tracking/KCFcpp/src/kcftracker.cpp b/roborts_tracking/KCFcpp/src/kcftracker.cpp new file mode 100644 index 00000000..7012f973 --- /dev/null +++ b/roborts_tracking/KCFcpp/src/kcftracker.cpp @@ -0,0 +1,519 @@ +/* + +Tracker based on Kernelized Correlation Filter (KCF) [1] and Circulant Structure with Kernels (CSK) [2]. +CSK is implemented by using raw gray level features, since it is a single-channel filter. +KCF is implemented by using HOG features (the default), since it extends CSK to multiple channels. + +[1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, +"High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015. + +[2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, +"Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012. + +Authors: Joao Faro, Christian Bailer, Joao F. Henriques +Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt +Institute of Systems and Robotics - University of Coimbra / Department Augmented Vision DFKI + + +Constructor parameters, all boolean: + hog: use HOG features (default), otherwise use raw pixels + fixed_window: fix window size (default), otherwise use ROI size (slower but more accurate) + multiscale: use multi-scale tracking (default; cannot be used with fixed_window = true) + +Default values are set for all properties of the tracker depending on the above choices. +Their values can be customized further before calling init(): + interp_factor: linear interpolation factor for adaptation + sigma: gaussian kernel bandwidth + lambda: regularization + cell_size: HOG cell size + padding: area surrounding the target, relative to its size + output_sigma_factor: bandwidth of gaussian target + template_size: template size in pixels, 0 to use ROI size + scale_step: scale step for multi-scale estimation, 1 to disable it + scale_weight: to downweight detection scores of other scales for added stability + +For speed, the value (template_size/cell_size) should be a power of 2 or a product of small prime numbers. + +Inputs to init(): + image is the initial frame. + roi is a cv::Rect with the target positions in the initial frame + +Inputs to update(): + image is the current frame. + +Outputs of update(): + cv::Rect with target positions for the current frame + + +By downloading, copying, installing or using the software you agree to this license. +If you do not agree to this license, do not download, install, +copy or use the software. + + + License Agreement + For Open Source Computer Vision Library + (3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the names of the copyright holders nor the names of the contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +This software is provided by the copyright holders and contributors "as is" and +any express or implied warranties, including, but not limited to, the implied +warranties of merchantability and fitness for a particular purpose are disclaimed. +In no event shall copyright holders or contributors be liable for any direct, +indirect, incidental, special, exemplary, or consequential damages +(including, but not limited to, procurement of substitute goods or services; +loss of use, data, or profits; or business interruption) however caused +and on any theory of liability, whether in contract, strict liability, +or tort (including negligence or otherwise) arising in any way out of +the use of this software, even if advised of the possibility of such damage. + */ + +#ifndef _KCFTRACKER_HEADERS +#include "kcftracker.hpp" +#include "ffttools.hpp" +#include "recttools.hpp" +#include "fhog.hpp" +#include "labdata.hpp" +#endif + +// Constructor +KCFTracker::KCFTracker(bool hog, bool fixed_window, bool multiscale, bool lab) +{ + + // Parameters equal in all cases + lambda = 0.0001; + padding = 2.5; + //output_sigma_factor = 0.1; + output_sigma_factor = 0.125; + + + if (hog) { // HOG + // VOT + interp_factor = 0.012; + sigma = 0.6; + // TPAMI + //interp_factor = 0.02; + //sigma = 0.5; + cell_size = 4; + _hogfeatures = true; + + if (lab) { + interp_factor = 0.005; + sigma = 0.4; + //output_sigma_factor = 0.025; + output_sigma_factor = 0.1; + + _labfeatures = true; + _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); + cell_sizeQ = cell_size*cell_size; + } + else{ + _labfeatures = false; + } + } + else { // RAW + interp_factor = 0.075; + sigma = 0.2; + cell_size = 1; + _hogfeatures = false; + + if (lab) { + printf("Lab features are only used with HOG features.\n"); + _labfeatures = false; + } + } + + + if (multiscale) { // multiscale + template_size = 96; + //template_size = 100; + scale_step = 1.05; + scale_weight = 0.95; + if (!fixed_window) { + //printf("Multiscale does not support non-fixed window.\n"); + fixed_window = true; + } + } + else if (fixed_window) { // fit correction without multiscale + template_size = 96; + //template_size = 100; + scale_step = 1; + } + else { + template_size = 1; + scale_step = 1; + } +} + +// Initialize tracker +void KCFTracker::init(const cv::Rect &roi, cv::Mat image) +{ + _roi = roi; + assert(roi.width >= 0 && roi.height >= 0); + _tmpl = getFeatures(image, 1); + _prob = createGaussianPeak(size_patch[0], size_patch[1]); + _alphaf = cv::Mat(size_patch[0], size_patch[1], CV_32FC2, float(0)); + //_num = cv::Mat(size_patch[0], size_patch[1], CV_32FC2, float(0)); + //_den = cv::Mat(size_patch[0], size_patch[1], CV_32FC2, float(0)); + train(_tmpl, 1.0); // train with initial frame + } +// Update position based on the new frame +cv::Rect KCFTracker::update(cv::Mat image) +{ + if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; + if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; + if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; + if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; + + float cx = _roi.x + _roi.width / 2.0f; + float cy = _roi.y + _roi.height / 2.0f; + + + float peak_value; + cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); + + if (scale_step != 1) { + // Test at a smaller _scale + float new_peak_value; + cv::Point2f new_res = detect(_tmpl, getFeatures(image, 0, 1.0f / scale_step), new_peak_value); + + if (scale_weight * new_peak_value > peak_value) { + res = new_res; + peak_value = new_peak_value; + _scale /= scale_step; + _roi.width /= scale_step; + _roi.height /= scale_step; + } + + // Test at a bigger _scale + new_res = detect(_tmpl, getFeatures(image, 0, scale_step), new_peak_value); + + if (scale_weight * new_peak_value > peak_value) { + res = new_res; + peak_value = new_peak_value; + _scale *= scale_step; + _roi.width *= scale_step; + _roi.height *= scale_step; + } + } + + // Adjust by cell size and _scale + _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); + _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); + + if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1; + if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1; + if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2; + if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2; + + assert(_roi.width >= 0 && _roi.height >= 0); + cv::Mat x = getFeatures(image, 0); + train(x, interp_factor); + + return _roi; +} + + +// Detect object in the current frame. +cv::Point2f KCFTracker::detect(cv::Mat z, cv::Mat x, float &peak_value) +{ + using namespace FFTTools; + + cv::Mat k = gaussianCorrelation(x, z); + cv::Mat res = (real(fftd(complexMultiplication(_alphaf, fftd(k)), true))); + + //minMaxLoc only accepts doubles for the peak, and integer points for the coordinates + cv::Point2i pi; + double pv; + cv::minMaxLoc(res, NULL, &pv, NULL, &pi); + peak_value = (float) pv; + + //subpixel peak estimation, coordinates will be non-integer + cv::Point2f p((float)pi.x, (float)pi.y); + + if (pi.x > 0 && pi.x < res.cols-1) { + p.x += subPixelPeak(res.at(pi.y, pi.x-1), peak_value, res.at(pi.y, pi.x+1)); + } + + if (pi.y > 0 && pi.y < res.rows-1) { + p.y += subPixelPeak(res.at(pi.y-1, pi.x), peak_value, res.at(pi.y+1, pi.x)); + } + + p.x -= (res.cols) / 2; + p.y -= (res.rows) / 2; + + return p; +} + +// train tracker with a single image +void KCFTracker::train(cv::Mat x, float train_interp_factor) +{ + using namespace FFTTools; + + cv::Mat k = gaussianCorrelation(x, x); + cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda)); + + _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; + _alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf; + + + /*cv::Mat kf = fftd(gaussianCorrelation(x, x)); + cv::Mat num = complexMultiplication(kf, _prob); + cv::Mat den = complexMultiplication(kf, kf + lambda); + + _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; + _num = (1 - train_interp_factor) * _num + (train_interp_factor) * num; + _den = (1 - train_interp_factor) * _den + (train_interp_factor) * den; + + _alphaf = complexDivision(_num, _den);*/ + +} + +// Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window). +cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2) +{ + using namespace FFTTools; + cv::Mat c = cv::Mat( cv::Size(size_patch[1], size_patch[0]), CV_32F, cv::Scalar(0) ); + // HOG features + if (_hogfeatures) { + cv::Mat caux; + cv::Mat x1aux; + cv::Mat x2aux; + for (int i = 0; i < size_patch[2]; i++) { + x1aux = x1.row(i); // Procedure do deal with cv::Mat multichannel bug + x1aux = x1aux.reshape(1, size_patch[0]); + x2aux = x2.row(i).reshape(1, size_patch[0]); + cv::mulSpectrums(fftd(x1aux), fftd(x2aux), caux, 0, true); + caux = fftd(caux, true); + rearrange(caux); + caux.convertTo(caux,CV_32F); + c = c + real(caux); + } + } + // Gray features + else { + cv::mulSpectrums(fftd(x1), fftd(x2), c, 0, true); + c = fftd(c, true); + rearrange(c); + c = real(c); + } + cv::Mat d; + cv::max(( (cv::sum(x1.mul(x1))[0] + cv::sum(x2.mul(x2))[0])- 2. * c) / (size_patch[0]*size_patch[1]*size_patch[2]) , 0, d); + + cv::Mat k; + cv::exp((-d / (sigma * sigma)), k); + return k; +} + +// Create Gaussian Peak. Function called only in the first frame. +cv::Mat KCFTracker::createGaussianPeak(int sizey, int sizex) +{ + cv::Mat_ res(sizey, sizex); + + int syh = (sizey) / 2; + int sxh = (sizex) / 2; + + float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; + float mult = -0.5 / (output_sigma * output_sigma); + + for (int i = 0; i < sizey; i++) + for (int j = 0; j < sizex; j++) + { + int ih = i - syh; + int jh = j - sxh; + res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); + } + return FFTTools::fftd(res); +} + +// Obtain sub-window from image, with replication-padding and extract features +cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scale_adjust) +{ + cv::Rect extracted_roi; + + float cx = _roi.x + _roi.width / 2; + float cy = _roi.y + _roi.height / 2; + + if (inithann) { + int padded_w = _roi.width * padding; + int padded_h = _roi.height * padding; + + if (template_size > 1) { // Fit largest dimension to the given template size + if (padded_w >= padded_h) //fit to width + _scale = padded_w / (float) template_size; + else + _scale = padded_h / (float) template_size; + + _tmpl_sz.width = padded_w / _scale; + _tmpl_sz.height = padded_h / _scale; + } + else { //No template size given, use ROI size + _tmpl_sz.width = padded_w; + _tmpl_sz.height = padded_h; + _scale = 1; + // original code from paper: + /*if (sqrt(padded_w * padded_h) >= 100) { //Normal size + _tmpl_sz.width = padded_w; + _tmpl_sz.height = padded_h; + _scale = 1; + } + else { //ROI is too big, track at half size + _tmpl_sz.width = padded_w / 2; + _tmpl_sz.height = padded_h / 2; + _scale = 2; + }*/ + } + + if (_hogfeatures) { + // Round to cell size and also make it even + _tmpl_sz.width = ( ( (int)(_tmpl_sz.width / (2 * cell_size)) ) * 2 * cell_size ) + cell_size*2; + _tmpl_sz.height = ( ( (int)(_tmpl_sz.height / (2 * cell_size)) ) * 2 * cell_size ) + cell_size*2; + } + else { //Make number of pixels even (helps with some logic involving half-dimensions) + _tmpl_sz.width = (_tmpl_sz.width / 2) * 2; + _tmpl_sz.height = (_tmpl_sz.height / 2) * 2; + } + } + + extracted_roi.width = scale_adjust * _scale * _tmpl_sz.width; + extracted_roi.height = scale_adjust * _scale * _tmpl_sz.height; + + // center roi with new size + extracted_roi.x = cx - extracted_roi.width / 2; + extracted_roi.y = cy - extracted_roi.height / 2; + + cv::Mat FeaturesMap; + cv::Mat z = RectTools::subwindow(image, extracted_roi, cv::BORDER_REPLICATE); + + if (z.cols != _tmpl_sz.width || z.rows != _tmpl_sz.height) { + cv::resize(z, z, _tmpl_sz); + } + + // HOG features + if (_hogfeatures) { + IplImage z_ipl = z; + CvLSVMFeatureMapCaskade *map; + getFeatureMaps(&z_ipl, cell_size, &map); + normalizeAndTruncate(map,0.2f); + PCAFeatureMaps(map); + size_patch[0] = map->sizeY; + size_patch[1] = map->sizeX; + size_patch[2] = map->numFeatures; + + FeaturesMap = cv::Mat(cv::Size(map->numFeatures,map->sizeX*map->sizeY), CV_32F, map->map); // Procedure do deal with cv::Mat multichannel bug + FeaturesMap = FeaturesMap.t(); + freeFeatureMapObject(&map); + + // Lab features + if (_labfeatures) { + cv::Mat imgLab; + cvtColor(z, imgLab, CV_BGR2Lab); + unsigned char *input = (unsigned char*)(imgLab.data); + + // Sparse output vector + cv::Mat outputLab = cv::Mat(_labCentroids.rows, size_patch[0]*size_patch[1], CV_32F, float(0)); + + int cntCell = 0; + // Iterate through each cell + for (int cY = cell_size; cY < z.rows-cell_size; cY+=cell_size){ + for (int cX = cell_size; cX < z.cols-cell_size; cX+=cell_size){ + // Iterate through each pixel of cell (cX,cY) + for(int y = cY; y < cY+cell_size; ++y){ + for(int x = cX; x < cX+cell_size; ++x){ + // Lab components for each pixel + float l = (float)input[(z.cols * y + x) * 3]; + float a = (float)input[(z.cols * y + x) * 3 + 1]; + float b = (float)input[(z.cols * y + x) * 3 + 2]; + + // Iterate trough each centroid + float minDist = FLT_MAX; + int minIdx = 0; + float *inputCentroid = (float*)(_labCentroids.data); + for(int k = 0; k < _labCentroids.rows; ++k){ + float dist = ( (l - inputCentroid[3*k]) * (l - inputCentroid[3*k]) ) + + ( (a - inputCentroid[3*k+1]) * (a - inputCentroid[3*k+1]) ) + + ( (b - inputCentroid[3*k+2]) * (b - inputCentroid[3*k+2]) ); + if(dist < minDist){ + minDist = dist; + minIdx = k; + } + } + // Store result at output + outputLab.at(minIdx, cntCell) += 1.0 / cell_sizeQ; + //((float*) outputLab.data)[minIdx * (size_patch[0]*size_patch[1]) + cntCell] += 1.0 / cell_sizeQ; + } + } + cntCell++; + } + } + // Update size_patch[2] and add features to FeaturesMap + size_patch[2] += _labCentroids.rows; + FeaturesMap.push_back(outputLab); + } + } + else { + FeaturesMap = RectTools::getGrayImage(z); + FeaturesMap -= (float) 0.5; // In Paper; + size_patch[0] = z.rows; + size_patch[1] = z.cols; + size_patch[2] = 1; + } + + if (inithann) { + createHanningMats(); + } + FeaturesMap = hann.mul(FeaturesMap); + return FeaturesMap; +} + +// Initialize Hanning window. Function called only in the first frame. +void KCFTracker::createHanningMats() +{ + cv::Mat hann1t = cv::Mat(cv::Size(size_patch[1],1), CV_32F, cv::Scalar(0)); + cv::Mat hann2t = cv::Mat(cv::Size(1,size_patch[0]), CV_32F, cv::Scalar(0)); + + for (int i = 0; i < hann1t.cols; i++) + hann1t.at (0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1))); + for (int i = 0; i < hann2t.rows; i++) + hann2t.at (i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1))); + + cv::Mat hann2d = hann2t * hann1t; + // HOG features + if (_hogfeatures) { + cv::Mat hann1d = hann2d.reshape(1,1); // Procedure do deal with cv::Mat multichannel bug + + hann = cv::Mat(cv::Size(size_patch[0]*size_patch[1], size_patch[2]), CV_32F, cv::Scalar(0)); + for (int i = 0; i < size_patch[2]; i++) { + for (int j = 0; j(i,j) = hann1d.at(0,j); + } + } + } + // Gray features + else { + hann = hann2d; + } +} + +// Calculate sub-pixel peak for one dimension +float KCFTracker::subPixelPeak(float left, float center, float right) +{ + float divisor = 2 * center - right - left; + + if (divisor == 0) + return 0; + + return 0.5 * (right - left) / divisor; +} diff --git a/roborts_tracking/KCFcpp/src/kcftracker.hpp b/roborts_tracking/KCFcpp/src/kcftracker.hpp new file mode 100644 index 00000000..4463f4a9 --- /dev/null +++ b/roborts_tracking/KCFcpp/src/kcftracker.hpp @@ -0,0 +1,151 @@ +/* + +Tracker based on Kernelized Correlation Filter (KCF) [1] and Circulant Structure with Kernels (CSK) [2]. +CSK is implemented by using raw gray level features, since it is a single-channel filter. +KCF is implemented by using HOG features (the default), since it extends CSK to multiple channels. + +[1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, +"High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015. + +[2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, +"Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012. + +Authors: Joao Faro, Christian Bailer, Joao F. Henriques +Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt +Institute of Systems and Robotics - University of Coimbra / Department Augmented Vision DFKI + + +Constructor parameters, all boolean: + hog: use HOG features (default), otherwise use raw pixels + fixed_window: fix window size (default), otherwise use ROI size (slower but more accurate) + multiscale: use multi-scale tracking (default; cannot be used with fixed_window = true) + +Default values are set for all properties of the tracker depending on the above choices. +Their values can be customized further before calling init(): + interp_factor: linear interpolation factor for adaptation + sigma: gaussian kernel bandwidth + lambda: regularization + cell_size: HOG cell size + padding: horizontal area surrounding the target, relative to its size + output_sigma_factor: bandwidth of gaussian target + template_size: template size in pixels, 0 to use ROI size + scale_step: scale step for multi-scale estimation, 1 to disable it + scale_weight: to downweight detection scores of other scales for added stability + +For speed, the value (template_size/cell_size) should be a power of 2 or a product of small prime numbers. + +Inputs to init(): + image is the initial frame. + roi is a cv::Rect with the target positions in the initial frame + +Inputs to update(): + image is the current frame. + +Outputs of update(): + cv::Rect with target positions for the current frame + + +By downloading, copying, installing or using the software you agree to this license. +If you do not agree to this license, do not download, install, +copy or use the software. + + + License Agreement + For Open Source Computer Vision Library + (3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the names of the copyright holders nor the names of the contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +This software is provided by the copyright holders and contributors "as is" and +any express or implied warranties, including, but not limited to, the implied +warranties of merchantability and fitness for a particular purpose are disclaimed. +In no event shall copyright holders or contributors be liable for any direct, +indirect, incidental, special, exemplary, or consequential damages +(including, but not limited to, procurement of substitute goods or services; +loss of use, data, or profits; or business interruption) however caused +and on any theory of liability, whether in contract, strict liability, +or tort (including negligence or otherwise) arising in any way out of +the use of this software, even if advised of the possibility of such damage. + */ + +#pragma once + +#include "tracker.h" + +#ifndef _OPENCV_KCFTRACKER_HPP_ +#define _OPENCV_KCFTRACKER_HPP_ +#endif + +class KCFTracker : public Tracker +{ +public: + // Constructor + KCFTracker(bool hog = true, bool fixed_window = true, bool multiscale = true, bool lab = true); + + // Initialize tracker + virtual void init(const cv::Rect &roi, cv::Mat image); + + // Update position based on the new frame + virtual cv::Rect update(cv::Mat image); + + float interp_factor; // linear interpolation factor for adaptation + float sigma; // gaussian kernel bandwidth + float lambda; // regularization + int cell_size; // HOG cell size + int cell_sizeQ; // cell size^2, to avoid repeated operations + float padding; // extra area surrounding the target + float output_sigma_factor; // bandwidth of gaussian target + int template_size; // template size + float scale_step; // scale step for multi-scale estimation + float scale_weight; // to downweight detection scores of other scales for added stability + +protected: + // Detect object in the current frame. + cv::Point2f detect(cv::Mat z, cv::Mat x, float &peak_value); + + // train tracker with a single image + void train(cv::Mat x, float train_interp_factor); + + // Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window). + cv::Mat gaussianCorrelation(cv::Mat x1, cv::Mat x2); + + // Create Gaussian Peak. Function called only in the first frame. + cv::Mat createGaussianPeak(int sizey, int sizex); + + // Obtain sub-window from image, with replication-padding and extract features + cv::Mat getFeatures(const cv::Mat & image, bool inithann, float scale_adjust = 1.0f); + + // Initialize Hanning window. Function called only in the first frame. + void createHanningMats(); + + // Calculate sub-pixel peak for one dimension + float subPixelPeak(float left, float center, float right); + + cv::Mat _alphaf; + cv::Mat _prob; + cv::Mat _tmpl; + cv::Mat _num; + cv::Mat _den; + cv::Mat _labCentroids; + +private: + int size_patch[3]; + cv::Mat hann; + cv::Size _tmpl_sz; + float _scale; + int _gaussian_size; + bool _hogfeatures; + bool _labfeatures; +}; diff --git a/roborts_tracking/KCFcpp/src/labdata.hpp b/roborts_tracking/KCFcpp/src/labdata.hpp new file mode 100644 index 00000000..26e7a65f --- /dev/null +++ b/roborts_tracking/KCFcpp/src/labdata.hpp @@ -0,0 +1,17 @@ +const int nClusters = 15; +float data[nClusters][3] = { +{161.317504, 127.223401, 128.609333}, +{142.922425, 128.666965, 127.532319}, +{67.879757, 127.721830, 135.903311}, +{92.705062, 129.965717, 137.399500}, +{120.172257, 128.279647, 127.036493}, +{195.470568, 127.857070, 129.345415}, +{41.257102, 130.059468, 132.675336}, +{12.014861, 129.480555, 127.064714}, +{226.567086, 127.567831, 136.345727}, +{154.664210, 131.676606, 156.481669}, +{121.180447, 137.020793, 153.433743}, +{87.042204, 137.211742, 98.614874}, +{113.809537, 106.577104, 157.818094}, +{81.083293, 170.051905, 148.904079}, +{45.015485, 138.543124, 102.402528}}; \ No newline at end of file diff --git a/roborts_tracking/KCFcpp/src/recttools.hpp b/roborts_tracking/KCFcpp/src/recttools.hpp new file mode 100644 index 00000000..780ad328 --- /dev/null +++ b/roborts_tracking/KCFcpp/src/recttools.hpp @@ -0,0 +1,140 @@ +/* +Author: Christian Bailer +Contact address: Christian.Bailer@dfki.de +Department Augmented Vision DFKI + + License Agreement + For Open Source Computer Vision Library + (3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the names of the copyright holders nor the names of the contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +This software is provided by the copyright holders and contributors "as is" and +any express or implied warranties, including, but not limited to, the implied +warranties of merchantability and fitness for a particular purpose are disclaimed. +In no event shall copyright holders or contributors be liable for any direct, +indirect, incidental, special, exemplary, or consequential damages +(including, but not limited to, procurement of substitute goods or services; +loss of use, data, or profits; or business interruption) however caused +and on any theory of liability, whether in contract, strict liability, +or tort (including negligence or otherwise) arising in any way out of +the use of this software, even if advised of the possibility of such damage. +*/ + +#pragma once + +//#include +#include + +#ifndef _OPENCV_RECTTOOLS_HPP_ +#define _OPENCV_RECTTOOLS_HPP_ +#endif + +namespace RectTools +{ + +template +inline cv::Vec center(const cv::Rect_ &rect) +{ + return cv::Vec (rect.x + rect.width / (t) 2, rect.y + rect.height / (t) 2); +} + +template +inline t x2(const cv::Rect_ &rect) +{ + return rect.x + rect.width; +} + +template +inline t y2(const cv::Rect_ &rect) +{ + return rect.y + rect.height; +} + +template +inline void resize(cv::Rect_ &rect, float scalex, float scaley = 0) +{ + if (!scaley)scaley = scalex; + rect.x -= rect.width * (scalex - 1.f) / 2.f; + rect.width *= scalex; + + rect.y -= rect.height * (scaley - 1.f) / 2.f; + rect.height *= scaley; + +} + +template +inline void limit(cv::Rect_ &rect, cv::Rect_ limit) +{ + if (rect.x + rect.width > limit.x + limit.width)rect.width = (limit.x + limit.width - rect.x); + if (rect.y + rect.height > limit.y + limit.height)rect.height = (limit.y + limit.height - rect.y); + if (rect.x < limit.x) + { + rect.width -= (limit.x - rect.x); + rect.x = limit.x; + } + if (rect.y < limit.y) + { + rect.height -= (limit.y - rect.y); + rect.y = limit.y; + } + if(rect.width<0)rect.width=0; + if(rect.height<0)rect.height=0; +} + +template +inline void limit(cv::Rect_ &rect, t width, t height, t x = 0, t y = 0) +{ + limit(rect, cv::Rect_ (x, y, width, height)); +} + +template +inline cv::Rect getBorder(const cv::Rect_ &original, cv::Rect_ & limited) +{ + cv::Rect_ res; + res.x = limited.x - original.x; + res.y = limited.y - original.y; + res.width = x2(original) - x2(limited); + res.height = y2(original) - y2(limited); + assert(res.x >= 0 && res.y >= 0 && res.width >= 0 && res.height >= 0); + return res; +} + +inline cv::Mat subwindow(const cv::Mat &in, const cv::Rect & window, int borderType = cv::BORDER_CONSTANT) +{ + cv::Rect cutWindow = window; + RectTools::limit(cutWindow, in.cols, in.rows); + if (cutWindow.height <= 0 || cutWindow.width <= 0)assert(0); //return cv::Mat(window.height,window.width,in.type(),0) ; + cv::Rect border = RectTools::getBorder(window, cutWindow); + cv::Mat res = in(cutWindow); + + if (border != cv::Rect(0, 0, 0, 0)) + { + cv::copyMakeBorder(res, res, border.y, border.height, border.x, border.width, borderType); + } + return res; +} + +inline cv::Mat getGrayImage(cv::Mat img) +{ + cv::cvtColor(img, img, CV_BGR2GRAY); + img.convertTo(img, CV_32F, 1 / 255.f); + return img; +} + +} + + + diff --git a/roborts_tracking/KCFcpp/src/runtracker.cpp b/roborts_tracking/KCFcpp/src/runtracker.cpp new file mode 100644 index 00000000..e44c9260 --- /dev/null +++ b/roborts_tracking/KCFcpp/src/runtracker.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include + +#include +#include + +#include "kcftracker.hpp" + +#include + +using namespace std; +using namespace cv; + +int main(int argc, char* argv[]){ + + if (argc > 5) return -1; + + bool HOG = true; + bool FIXEDWINDOW = false; + bool MULTISCALE = true; + bool SILENT = true; + bool LAB = false; + + for(int i = 0; i < argc; i++){ + if ( strcmp (argv[i], "hog") == 0 ) + HOG = true; + if ( strcmp (argv[i], "fixed_window") == 0 ) + FIXEDWINDOW = true; + if ( strcmp (argv[i], "singlescale") == 0 ) + MULTISCALE = false; + if ( strcmp (argv[i], "show") == 0 ) + SILENT = false; + if ( strcmp (argv[i], "lab") == 0 ){ + LAB = true; + HOG = true; + } + if ( strcmp (argv[i], "gray") == 0 ) + HOG = false; + } + + // Create KCFTracker object + KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB); + + // Frame readed + Mat frame; + + // Tracker results + Rect result; + + // Path to list.txt + ifstream listFile; + string fileName = "images.txt"; + listFile.open(fileName); + + // Read groundtruth for the 1st frame + ifstream groundtruthFile; + string groundtruth = "region.txt"; + groundtruthFile.open(groundtruth); + string firstLine; + getline(groundtruthFile, firstLine); + groundtruthFile.close(); + + istringstream ss(firstLine); + + // Read groundtruth like a dumb + float x1, y1, x2, y2, x3, y3, x4, y4; + char ch; + ss >> x1; + ss >> ch; + ss >> y1; + ss >> ch; + ss >> x2; + ss >> ch; + ss >> y2; + ss >> ch; + ss >> x3; + ss >> ch; + ss >> y3; + ss >> ch; + ss >> x4; + ss >> ch; + ss >> y4; + + // Using min and max of X and Y for groundtruth rectangle + float xMin = min(x1, min(x2, min(x3, x4))); + float yMin = min(y1, min(y2, min(y3, y4))); + float width = max(x1, max(x2, max(x3, x4))) - xMin; + float height = max(y1, max(y2, max(y3, y4))) - yMin; + + + // Read Images + ifstream listFramesFile; + string listFrames = "images.txt"; + listFramesFile.open(listFrames); + string frameName; + + + // Write Results + ofstream resultsFile; + string resultsPath = "output.txt"; + resultsFile.open(resultsPath); + + // Frame counter + int nFrames = 0; + + + while ( getline(listFramesFile, frameName) ){ + frameName = frameName; + + // Read each frame from the list + frame = imread(frameName, CV_LOAD_IMAGE_COLOR); + + // First frame, give the groundtruth to the tracker + if (nFrames == 0) { + tracker.init( Rect(xMin, yMin, width, height), frame ); + rectangle( frame, Point( xMin, yMin ), Point( xMin+width, yMin+height), Scalar( 0, 255, 255 ), 1, 8 ); + resultsFile << xMin << "," << yMin << "," << width << "," << height << endl; + } + // Update + else{ + result = tracker.update(frame); + rectangle( frame, Point( result.x, result.y ), Point( result.x+result.width, result.y+result.height), Scalar( 0, 255, 255 ), 1, 8 ); + resultsFile << result.x << "," << result.y << "," << result.width << "," << result.height << endl; + } + + nFrames++; + + if (!SILENT){ + imshow("Image", frame); + waitKey(1); + } + } + resultsFile.close(); + + listFile.close(); + +} diff --git a/roborts_tracking/KCFcpp/src/tracker.h b/roborts_tracking/KCFcpp/src/tracker.h new file mode 100644 index 00000000..41b5ea3d --- /dev/null +++ b/roborts_tracking/KCFcpp/src/tracker.h @@ -0,0 +1,39 @@ +/* + * File: BasicTracker.h + * Author: Joao F. Henriques, Joao Faro, Christian Bailer + * Contact address: henriques@isr.uc.pt, joaopfaro@gmail.com, Christian.Bailer@dfki.de + * Instute of Systems and Robotics- University of COimbra / Department Augmented Vision DFKI + * + * This source code is provided for for research purposes only. For a commercial license or a different use case please contact us. + * You are not allowed to publish the unmodified sourcecode on your own e.g. on your webpage. Please refer to the official download page instead. + * If you want to publish a modified/extended version e.g. because you wrote a publication with a modified version of the sourcecode you need our + * permission (Please contact us for the permission). + * + * We reserve the right to change the license of this sourcecode anytime to BSD, GPL or LGPL. + * By using the sourcecode you agree to possible restrictions and requirements of these three license models so that the license can be changed + * anytime without you knowledge. + */ + + + +#pragma once + +#include +#include + +class Tracker +{ +public: + Tracker() {} + virtual ~Tracker() { } + + virtual void init(const cv::Rect &roi, cv::Mat image) = 0; + virtual cv::Rect update( cv::Mat image)=0; + + +protected: + cv::Rect_ _roi; +}; + + + diff --git a/roborts_tracking/package.xml b/roborts_tracking/package.xml new file mode 100755 index 00000000..5826fa37 --- /dev/null +++ b/roborts_tracking/package.xml @@ -0,0 +1,24 @@ + + roborts_tracking + 1.0.0 + + The roborts_tracking package provides object tracking for RoboMaster AI Robot + + RoboMaster + RoboMaster + GPL 3.0 + https://github.com/RoboMaster/RoboRTS + + catkin + + roscpp + rospy + tf + roborts_msgs + + roscpp + rospy + tf + roborts_msgs + + diff --git a/roborts_tracking/tracking_test.cpp b/roborts_tracking/tracking_test.cpp new file mode 100644 index 00000000..99276dca --- /dev/null +++ b/roborts_tracking/tracking_test.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "roborts_msgs/GimbalAngle.h" +#include "roborts_msgs/GimbalRate.h" + + +#include "tracking_utility.h" + +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "KCFcpp/src/kcftracker.hpp" + +#define HOG 1 +#define FIXEDWINDOW 1 +#define MULTISCALE 1 +#define LAB 1 + +typedef std::chrono::time_point timer; +typedef std::chrono::duration duration; + +using namespace std; +using namespace cv; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "roborts_tracking_node"); + ros::NodeHandle nh; + auto pub= nh.advertise("cmd_gimbal_rate", 30); + const char winName[]="My Camera"; + char message1[100]; + char message2[100]; + Rect roi(0,0,0,0); + TrackingUtility tu; + KCFTracker *tracker = NULL; + + cv::namedWindow(winName,1); + cv::setMouseCallback(winName,TrackingUtility::mouseCallback, (void*)&tu); + + + VideoCapture video(0); + int img_width = 640; + int img_height = 480; + video.set(CV_CAP_PROP_FRAME_WIDTH,img_width); + video.set(CV_CAP_PROP_FRAME_HEIGHT,img_height); + img_width = video.get(CV_CAP_PROP_FRAME_WIDTH); + img_height = video.get(CV_CAP_PROP_FRAME_HEIGHT); + + if (!video.isOpened()) { + cout << "cannot read video!" << endl; + return -1; + } + //可选BOOSTING, MIL, KCF, TLD, MEDIANFLOW, or GOTURN + + while(ros::ok()) + { + char c = cv::waitKey(10); + if(c==27) + { + if(tracker != NULL) + { + delete tracker; + tracker = NULL; + } + break; // Quit if ESC is pressed + } + + tu.getKey(c); //Internal states will be updated based on key pressed. + + Mat frame; + if(video.read(frame)){ + int dx = 0; + int dy = 0; + int yawRate = 0; + int pitchRate = 0; + timer trackerStartTime, trackerFinishTime; + duration trackerTimeDiff; + roborts_msgs::GimbalRate gimbal_rate; + roborts_msgs::GimbalAngle gimbal_angle; + int k = 1920/img_width; + switch(tu.getState()) + { + case TrackingUtility::STATE_IDLE: + roi = tu.getROI(); + sprintf(message2, "Please select ROI and press g"); + break; + + case TrackingUtility::STATE_INIT: + cout << "g pressed, initialize tracker" << endl; + sprintf(message2, "g pressed, initialize tracker"); + roi = tu.getROI(); + tracker = new KCFTracker(HOG, FIXEDWINDOW, MULTISCALE, LAB); + tracker->init(roi, frame); + tu.startTracker(); + break; + + case TrackingUtility::STATE_ONGOING: + trackerStartTime = std::chrono::high_resolution_clock::now(); + roi = tracker->update(frame); + trackerFinishTime = std::chrono::high_resolution_clock::now(); + trackerTimeDiff = trackerFinishTime - trackerStartTime; + sprintf(message2, "Tracking: bounding box update time = %.2f ms\n", trackerTimeDiff.count()*1000.0); + + // send gimbal speed command + dx = (int)(roi.x + roi.width/2 - img_width/2); + dy = (int)(roi.y + roi.height/2 - img_height/2); + + yawRate = -dx; + pitchRate = dy; + cout<<"yaw_rate:"<500/k) { + yawRate = ((yawRate>0)?1:-1)*500/k; + } + + if(abs(pitchRate) < 10/k) + { + pitchRate = 0; + } + else if(abs(pitchRate)>500/k) { + pitchRate = ((pitchRate>0)?1:-1)*500/k; + } + + + gimbal_rate.header.stamp = ros::Time::now(); + gimbal_rate.pitch_rate = pitchRate/180.*M_PI/110.*k; + gimbal_rate.yaw_rate = yawRate/180.*M_PI/160.*k; + pub.publish(gimbal_rate); + + break; + + case TrackingUtility::STATE_STOP: + cout << "s pressed, stop tracker" << endl; + sprintf(message2, "s pressed, stop tracker"); + delete tracker; + tracker = NULL; + tu.stopTracker(); + roi = tu.getROI(); + break; + + default: + break; + } + dx = roi.x + roi.width/2 - img_width/2; + dy = roi.y + roi.height/2 - img_height/2; + + cv::circle(frame, Point(img_width/2, img_height/2), 5, cv::Scalar(255,0,0), 2, 8); + if(roi.width != 0) + { + cv::circle(frame, Point(roi.x + roi.width/2, roi.y + roi.height/2), 3, cv::Scalar(0,0,255), 1, 8); + + cv::line(frame, Point(img_width/2, img_height/2), + Point(roi.x + roi.width/2, roi.y + roi.height/2), + cv::Scalar(0,255,255)); + } + + cv::rectangle(frame, roi, cv::Scalar(0,255,0), 1, 8, 0 ); + sprintf(message1,"dx=%04d, dy=%04d",dx, dy); + putText(frame, message1, Point2f(20,30), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,255,0)); + putText(frame, message2, Point2f(20,60), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,255,0)); + cv::imshow(winName, frame); + + } + + } + + if(tracker) + { + delete tracker; + } + + return 0; +} \ No newline at end of file diff --git a/roborts_tracking/tracking_utility.cpp b/roborts_tracking/tracking_utility.cpp new file mode 100644 index 00000000..6b3ca3f5 --- /dev/null +++ b/roborts_tracking/tracking_utility.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#include "tracking_utility.h" +#include +void TrackingUtility::mouseCallback(int event, int x, int y, int f, void *p) +{ + TrackingUtility *u = reinterpret_cast(p); + if(u->state != STATE_IDLE) + { + std::cout << "Currently tracking, press s key to stop" << std::endl; + return; + } + + switch(event) + { + case CV_EVENT_LBUTTONDOWN : + u->mouseClicked = true; + u->roiSelected = false; + u->P1 = cv::Point(x,y); + u->P2 = cv::Point(x,y); + break; + + case CV_EVENT_LBUTTONUP : + u->P2 = cv::Point(x,y); + u->mouseClicked=false; + if(u->P2 != u->P1) + { + u->roiSelected = true; + } + break; + + case CV_EVENT_MOUSEMOVE : + if(u->mouseClicked) + { + u->P2 = cv::Point(x,y); + } + break; + + default : + break; + } + + if(u->mouseClicked) + { + u->roi = cv::Rect(u->P1, u->P2); + printf("Current Region of Interest: %d, %d, %d, %d\n", u->roi.tl().x, u->roi.tl().y, u->roi.br().x, u->roi.br().y); + } +} + +cv::Rect TrackingUtility::getROI() +{ + return roi; +} + +TrackingUtility::TrackingState TrackingUtility::getState() +{ + return state; +} + +void TrackingUtility::startTracker() +{ + state = STATE_ONGOING; +} + +void TrackingUtility::stopTracker() +{ + state = STATE_IDLE; +} + +void TrackingUtility::getKey(char c) +{ + switch(c) + { + case 'g': + if( (state == STATE_IDLE) && (roiSelected == true)) + { + state = STATE_INIT; + } + break; + + case 's': + if( state == STATE_ONGOING ) + { + state = STATE_STOP; + roi = cv::Rect(0,0,0,0); //when we press s, should clear bounding box + } + break; + + default: + break; + } +} \ No newline at end of file diff --git a/roborts_tracking/tracking_utility.h b/roborts_tracking/tracking_utility.h new file mode 100644 index 00000000..4ba88eb4 --- /dev/null +++ b/roborts_tracking/tracking_utility.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * Copyright (C) 2019 RoboMaster. + * + * 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 . + ***************************************************************************/ + +#ifndef TEST_TRACKING_UTILITY_H +#define TEST_TRACKING_UTILITY_H +#include "opencv2/opencv.hpp" +/*! @brief + * The TrackingUtility Class handles simple start and stop tracking logic + */ +class TrackingUtility +{ +public: + TrackingUtility() + : P1(0,0), + P2(0,0), + roi(0,0,0,0), + mouseClicked(false), + roiSelected(false), + state(STATE_IDLE) + { + } + + typedef enum TrackingState + { + STATE_IDLE, + STATE_INIT, + STATE_ONGOING, + STATE_STOP + } TrackingState; + + static void mouseCallback(int event, int x, int y, int f, void *p); + + cv::Point P1; + cv::Point P2; + bool mouseClicked; + cv::Rect roi; + + /*! + * start_tracking is set true when you select a region and press 'g' + * is set to false when you press 's' + */ + bool roiSelected; + TrackingState state; + TrackingState getState(); + + void startTracker(); + void stopTracker(); + + cv::Rect getROI(); + void getKey(char c); +}; +#endif //TEST_TRACKING_UTILITY_H